Skip to content

Enables base motion on fetch robot with 1 anchor #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Nishantjannu wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Enables base motion on fetch robot with 1 anchor #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 5 additions & 5 deletions dedo/data/robots/fetch/fetch.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -155,7 +155,7 @@
</geometry>
</collision>
</link>
<joint name="torso_lift_joint" type="prismatic">
<joint name="torso_lift_joint" type="fixed">
<origin rpy="-6.123E-17 0 0" xyz="-0.086875 0 0.37743" />
<parent link="base_link" />
<child link="torso_lift_link" />
Expand Down Expand Up @@ -186,7 +186,7 @@
</geometry>
</collision>
</link>
<joint name="head_pan_joint" type="revolute">
<joint name="head_pan_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.053125 0 0.603001417713939" />
<parent link="torso_lift_link" />
<child link="head_pan_link" />
Expand Down Expand Up @@ -214,7 +214,7 @@
</geometry>
</collision>
</link>
<joint name="head_tilt_joint" type="revolute">
<joint name="head_tilt_joint" type="fixed">
<origin rpy="0 0.2617993877991494 0" xyz="0.14253 0 0.057999" />
<parent link="head_pan_link" />
<child link="head_tilt_link" />
Expand Down Expand Up @@ -481,7 +481,7 @@
</geometry>
</collision>
</link>
<joint name="r_gripper_finger_joint" type="prismatic">
<joint name="r_gripper_finger_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0.015425 0" />
<parent link="gripper_link" />
<child link="r_gripper_finger_link" />
Expand Down Expand Up @@ -509,7 +509,7 @@
</geometry>
</collision>
</link>
<joint name="l_gripper_finger_joint" type="prismatic">
<joint name="l_gripper_finger_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.015425 0" />
<parent link="gripper_link" />
<child link="l_gripper_finger_link" />
Expand Down
30 changes: 22 additions & 8 deletions dedo/demo_preset.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,10 +65,10 @@ def play(env, num_episodes, args):
height=args.cam_resolution)
if vidwriter is not None:
vidwriter.write(img[..., ::-1])
if args.debug:
viz_waypoints(env.sim, preset_wp['a'], (1, 0, 0, 1))
if 'b' in preset_wp:
viz_waypoints(env.sim, preset_wp['b'], (1, 0, 0, 0.5))
# if args.debug:
viz_waypoints(env.sim, preset_wp['a'], (1, 0, 1, 1))
if 'b' in preset_wp:
viz_waypoints(env.sim, preset_wp['b'], (1, 1, 0, 0.5))
# Need to step to get low-dim state from info.
step = 0
ctrl_freq = args.sim_freq / args.sim_steps_per_action
Expand All @@ -92,6 +92,7 @@ def play(env, num_episodes, args):
traj = merge_traj(pos_traj, pos_traj_b)
last_action = traj[-1]
traj_ori = preset_wp.get('a_theta', None)

if traj_ori is not None:
traj_ori = convert_all(np.array(traj_ori), 'theta_to_sin_cos')
n_repeats = traj.shape[0] // len(traj_ori)
Expand All @@ -101,6 +102,7 @@ def play(env, num_episodes, args):
assert (traj_ori.shape[1] == 6) # Euler sin,sin,sin,cos,cos,cos
traj = np.hstack([traj, traj_ori])

# print('traj', traj.shape)
gif_frames = []
rwds = []
print(f'# {args.env}:')
Expand All @@ -110,7 +112,10 @@ def play(env, num_episodes, args):

act = traj[step] if step < len(traj) else last_action

next_obs, rwd, done, info = env.step(act, unscaled=True)
print(step)

# Taking step- act is of size (3*num_anchors,1)
next_obs, rwd, done, info = env.step(act, unscaled=True, final_wp = last_action)
rwds.append(rwd)

if done and vidwriter is not None: # Record the internal steps after anchor drop
Expand All @@ -126,6 +131,7 @@ def play(env, num_episodes, args):
break

# if step > len(traj) + 50: break;

obs = next_obs

step += 1
Expand All @@ -143,7 +149,8 @@ def play(env, num_episodes, args):
def viz_waypoints(sim, waypoints, rgba):
waypoints = np.array(waypoints)
for waypoint in waypoints:
create_anchor_geom(sim, waypoint[:3], mass=0, rgba=rgba, use_collision=False)
anchor_id = create_anchor_geom(sim, waypoint[:3], mass=0, rgba=rgba, use_collision=False)
print("way point anchor: ", anchor_id)


def merge_traj(traj_a, traj_b):
Expand All @@ -164,17 +171,20 @@ def build_traj(env, preset_wp, left_or_right, anchor_idx, ctrl_freq, robot):
else:
anc_id = list(env.anchors.keys())[anchor_idx]
init_anc_pos = env.anchors[anc_id]['pos']
print(f'init_anc_pos {left_or_right}', init_anc_pos)
print(f'init_ee_pos {left_or_right}', init_anc_pos)
wp = np.array(preset_wp[left_or_right])
steps = (wp[:, -1] * ctrl_freq).round().astype(np.int32) # seconds -> ctrl steps
# print("steps", steps)

print('ATTENTION: Need to use scipy interpolate for preset trajs')
# exit(1)
# WARNING: old code below.

from scipy.interpolate import interp1d
wpt = np.concatenate([[init_anc_pos], wp[:, :3]], axis=0)
print("wpt", wpt)
ids = np.arange(wpt.shape[0])
print("ids", ids)
interp_type = 'linear'
# Creates the respective time interval for each way point
interp_i = []
Expand All @@ -188,8 +198,10 @@ def build_traj(env, preset_wp, left_or_right, anchor_idx, ctrl_freq, robot):
zi = interp1d(ids, wpt[:, 2], kind=interp_type)(interp_i)

traj = np.array([xi, yi, zi]).T
# print(traj.shape)

dv = (traj[1:] - traj[:-1]) # * ctrl_freq


# Calculating the avg velocity for each control step
chunks = []
Expand All @@ -211,6 +223,8 @@ def build_traj(env, preset_wp, left_or_right, anchor_idx, ctrl_freq, robot):
# Add the last point:
chunks = chunks + [[chunks[-1][-1]]]
velocities = np.concatenate(chunks, axis=0)
# print(velocities.shape)
# plot_traj(traj)

return traj, velocities

Expand All @@ -232,7 +246,7 @@ def plot_traj(traj):
def main(args):
np.set_printoptions(precision=4, linewidth=150, suppress=True)
kwargs = {'args': args}
env = gym.make(args.env, **kwargs)
env = gym.make(args.env, **kwargs) # unpacks the dictionary
env.seed(env.args.seed)
print('Created', args.task, 'with observation_space',
env.observation_space.shape, 'action_space', env.action_space.shape)
Expand Down
14 changes: 10 additions & 4 deletions dedo/envs/deform_env.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,12 @@ def __init__(self, args):
self.sim.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 0)
reset_bullet(args, self.sim, debug=args.debug)
self.food_packing = self.args.env.startswith('FoodPacking')
self.num_anchors = 1 if self.food_packing else 2
self.num_anchors = 1 # if self.food_packing else 2
print("Hi before calling load objects")
res = self.load_objects(self.sim, self.args, debug=True)
self.rigid_ids, self.deform_id, self.deform_obj, self.goal_pos = res
self.max_episode_len = self.args.max_episode_len
#################### REINFORCEMENT LEARNING ####################
# Define sizes of observation and action spaces.
self.gripper_lims = np.tile(np.concatenate(
[DeformEnv.WORKSPACE_BOX_SIZE * np.ones(3), # 3D pos
Expand Down Expand Up @@ -113,6 +115,7 @@ def get_texture_path(self, file_path):
return file_path

def load_objects(self, sim, args, debug):
# print("Hi env load objects start")
scene_name = self.args.task.lower()
if scene_name in ['hanggarment', 'bgarments', 'sewing','hangproccloth']:
scene_name = 'hangcloth' # same hanger for garments and cloths
Expand Down Expand Up @@ -291,6 +294,7 @@ def make_anchors(self):
for i in range(self.num_anchors): # make anchors
anchor_init_pos = self.args.anchor_init_pos if (i % 2) == 0 else \
self.args.other_anchor_init_pos
# creates an anchor and glues it to the deform object
anchor_id, anchor_pos, anchor_vertices = create_anchor(
self.sim, anchor_init_pos, i,
preset_dynamic_anchor_vertices, mesh)
Expand All @@ -312,17 +316,19 @@ def debug_viz_cent_loop(self):
# create_anchor_geom(self.sim, cent_pos, mass=0.0,
# rgba=(0, 1, 0.8, alpha), use_collision=False)

def step(self, action, unscaled=False):
def step(self, action, unscaled=False, final_wp = None):
if self.args.debug:
print('action', action)
if not unscaled:
assert self.action_space.contains(action)
assert ((np.abs(action) <= 1.0).all()), 'action must be in [-1, 1]'
action = action.reshape(self.num_anchors, -1)

if final_wp is not None:
final_wp = final_wp.reshape(self.num_anchors, -1)
# Step through physics simulation.

for sim_step in range(self.args.sim_steps_per_action):
self.do_action(action, unscaled)
self.do_action(action, unscaled, final_wp)
self.sim.stepSimulation()

# Get next obs, reward, done.
Expand Down
87 changes: 75 additions & 12 deletions dedo/envs/deform_robot_env.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
import gym
import numpy as np
import pybullet
from math import fabs

from ..utils.bullet_manipulator import BulletManipulator
from ..utils.init_utils import get_preset_properties
Expand Down Expand Up @@ -47,10 +48,14 @@ def unscale_pos(act, unscaled):
return act * DeformEnv.WORKSPACE_BOX_SIZE

def load_objects(self, sim, args, debug):
# call to load_objects of super class
res = super(DeformRobotEnv, self).load_objects(sim, args, debug)
data_path = os.path.join(os.path.split(__file__)[0], '..', 'data')
sim.setAdditionalSearchPath(data_path)
robot_info = ROBOT_INFO.get(f'franka{self.num_anchors:d}', None)

# robot_info = ROBOT_INFO.get(f'franka{self.num_anchors:d}', None)
robot_info = ROBOT_INFO.get('fetch', None)

robot_path = os.path.join(data_path, 'robots',
robot_info['file_name'])
if debug:
Expand All @@ -72,33 +77,85 @@ def load_objects(self, sim, args, debug):
left_fing_link_prefix='panda_hand_l_', left_joint_suffix='_l',
left_rest_arm_qpos=robot_info.get('left_rest_arm_qpos', None),
debug=debug)

return res

def make_anchors(self):
# incorrect name: not really making any new anchor
preset_dynamic_anchor_vertices = get_preset_properties(
DEFORM_INFO, self.deform_obj, 'deform_anchor_vertices')
_, mesh = get_mesh_data(self.sim, self.deform_id)
assert (preset_dynamic_anchor_vertices is not None)
for i in range(self.num_anchors): # make anchors
anchor_pos = np.array(mesh[preset_dynamic_anchor_vertices[i][0]])
for i in range(self.num_anchors): # make anchors
anchor_pos = np.array(mesh[preset_dynamic_anchor_vertices[i][0]]) # from the cloth definition (15,170)
print("location of anchor {} attached to garment before glue :".format(i), i, anchor_pos)
if not np.isfinite(anchor_pos).all():
print('anchor_pos not sane:', anchor_pos)
input('Press enter to exit')
exit(1)

link_id = self.robot.info.ee_link_id if i == 0 else \
self.robot.info.left_ee_link_id
self.sim.createSoftBodyAnchor(
self.deform_id, preset_dynamic_anchor_vertices[i][0],
self.robot.info.robot_id, link_id)

def do_action(self, action, unscaled=False):

# create a glue between the robot at given link position
# with the deform object at given vertex position
# THUS THERE IS NO CONCEPT OF GRASPING
# Already fixed - so now when end effector moves, the garment will move
# self.sim.createSoftBodyAnchor(
# self.deform_id, preset_dynamic_anchor_vertices[i][0],
# self.robot.info.robot_id, link_id)

def do_action(self, action, unscaled=False, final_wp = None):
# Note: action is in [-1,1], so we unscale pos (ori is sin,cos so ok).
action = action.reshape(self.num_anchors, -1)
ee_pos, ee_ori, _, _ = self.robot.get_ee_pos_ori_vel()
tgt_pos = DeformRobotEnv.unscale_pos(action[0, :3], unscaled)

print('ee_pos',ee_pos)
print('tgt_pos',tgt_pos)

if final_wp is not None:
final_wp = final_wp.reshape(self.num_anchors, -1)
final_pos = DeformRobotEnv.unscale_pos(final_wp[0, :3], unscaled)

# Calculate distance of robot base from final endpoint
# Assumption: Way-points (if multiple) are progressively further away from the bot
pos_rel_base, quat_rel_base = self.robot.get_relative_pose(pos = final_pos)

# base_state = self.sim.getLinkState(
# self.robot.info.robot_id, 0, computeLinkVelocity=0)
# base_pos = base_state[0]
# print(np.linalg.norm(pos_rel_base[:2]), self.robot.in_range, base_pos[:2])

print("error:", np.linalg.norm(ee_pos - final_pos))
print("pos_rel_base", np.linalg.norm(pos_rel_base[0:2]))

################### FOR MOVEMENT OF BASE ##########################

# If constraint created
if self.robot.base_cid:

# StopGap solution: due to fixed final difference in ee_pos and final_pos
error_th = 0.75

# Empirical value to bring robot within reach of target
robot_reach = 10.0
# Note: If IK solver is ideal - we would know that tgt pos is unreachable w/o
# moving robot if joint angles returned are out of range

if np.linalg.norm(pos_rel_base[0:2]) < robot_reach:
self.robot.in_range = 1
elif np.linalg.norm(pos_rel_base[0:2]) > robot_reach and self.robot.in_range is None:
self.robot.move_base(tgt_pos)
if (np.linalg.norm(ee_pos - final_pos) < error_th):
self.robot.in_range = 1

####################################################################

tgt_ee_ori = ee_ori if action.shape[-1] == 3 else action[0, 3:]
tgt_kwargs = {'ee_pos': tgt_pos, 'ee_ori': tgt_ee_ori,
'fing_dist': DeformRobotEnv.FING_DIST}

if self.num_anchors > 1: # dual-arm
res = self.robot.get_ee_pos_ori_vel(left=True)
left_ee_pos, left_ee_ori = res[0], res[1]
Expand All @@ -108,16 +165,22 @@ def do_action(self, action, unscaled=False):
tgt_kwargs.update({'left_ee_pos': left_tgt_pos,
'left_ee_ori': left_tgt_ee_ori,
'left_fing_dist': DeformRobotEnv.FING_DIST})
# so far we have a dict of tgt ee_pos for both arms
# now do inverse kinematics
tgt_qpos = self.robot.ee_pos_to_qpos(**tgt_kwargs)
# tgt_qpos = self.robot.get_qpos()
print("tgt_pose", tgt_qpos)
print("current_pose", self.robot.get_qpos())
n_slack = 1 # use > 1 if robot has trouble reaching the pose
sub_i = 0
ee_th = 0.01
diff = self.robot.get_qpos() - tgt_qpos
diff = np.abs(self.robot.get_qpos() - tgt_qpos)

while (diff > ee_th).any():
self.robot.move_to_qpos(
tgt_qpos, mode=pybullet.POSITION_CONTROL, kp=0.1, kd=1.0)
tgt_qpos, mode=pybullet.POSITION_CONTROL, kp=0.05, kd=7.0) # 0.05, 7.0
self.sim.stepSimulation()
diff = self.robot.get_qpos() - tgt_qpos
diff = np.abs(self.robot.get_qpos() - tgt_qpos)
sub_i += 1
if sub_i >= n_slack:
diff = np.zeros_like(diff) # set while loop to done
Expand All @@ -135,7 +198,7 @@ def make_final_steps(self):
print('final_action', final_action)
info = {'final_obs': []}
for sim_step in range(DeformEnv.STEPS_AFTER_DONE):
self.do_action(final_action, unscaled=True)
self.do_action(final_action, unscaled=True, final_wp = final_action)
self.sim.stepSimulation()
if sim_step % self.args.sim_steps_per_action == 0:
next_obs, _ = self.get_obs()
Expand Down
2 changes: 1 addition & 1 deletion dedo/utils/anchor_utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ def create_anchor(sim, anchor_pos, anchor_idx, preset_vertices, mesh,
anchor_vertices = None
mesh = np.array(mesh)
if use_preset and preset_vertices is not None:
anchor_vertices = preset_vertices[anchor_idx]
anchor_vertices = preset_vertices[anchor_idx] # location of anchor
anchor_pos = mesh[anchor_vertices].mean(axis=0)
elif use_closest:
anchor_pos, anchor_vertices = get_closest(anchor_pos, mesh)
Expand Down
Loading