diff --git a/dedo/data/robots/fetch/fetch.urdf b/dedo/data/robots/fetch/fetch.urdf index 47a61ab..cdc627b 100644 --- a/dedo/data/robots/fetch/fetch.urdf +++ b/dedo/data/robots/fetch/fetch.urdf @@ -155,7 +155,7 @@ - + @@ -186,7 +186,7 @@ - + @@ -214,7 +214,7 @@ - + @@ -481,7 +481,7 @@ - + @@ -509,7 +509,7 @@ - + diff --git a/dedo/demo_preset.py b/dedo/demo_preset.py index f14c670..8a1eaf0 100644 --- a/dedo/demo_preset.py +++ b/dedo/demo_preset.py @@ -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 @@ -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) @@ -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}:') @@ -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 @@ -126,6 +131,7 @@ def play(env, num_episodes, args): break # if step > len(traj) + 50: break; + obs = next_obs step += 1 @@ -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): @@ -164,9 +171,10 @@ 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) @@ -174,7 +182,9 @@ def build_traj(env, preset_wp, left_or_right, anchor_idx, ctrl_freq, robot): 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 = [] @@ -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 = [] @@ -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 @@ -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) diff --git a/dedo/envs/deform_env.py b/dedo/envs/deform_env.py index 1ce4100..26cfce6 100644 --- a/dedo/envs/deform_env.py +++ b/dedo/envs/deform_env.py @@ -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 @@ -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 @@ -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) @@ -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. diff --git a/dedo/envs/deform_robot_env.py b/dedo/envs/deform_robot_env.py index c4bdf18..8c6194c 100644 --- a/dedo/envs/deform_robot_env.py +++ b/dedo/envs/deform_robot_env.py @@ -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 @@ -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: @@ -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] @@ -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 @@ -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() diff --git a/dedo/utils/anchor_utils.py b/dedo/utils/anchor_utils.py index 07ae867..6081c5c 100644 --- a/dedo/utils/anchor_utils.py +++ b/dedo/utils/anchor_utils.py @@ -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) diff --git a/dedo/utils/bullet_manipulator.py b/dedo/utils/bullet_manipulator.py index ad51a53..c5b6c42 100644 --- a/dedo/utils/bullet_manipulator.py +++ b/dedo/utils/bullet_manipulator.py @@ -104,31 +104,44 @@ def __init__(self, sim, robot_desc_file, ee_joint_name, ee_link_name, # Create a constraint for the mobile manipulator to stay on the floor. # Note this constraint is not as rigid as using use_fixed_base while # loading, but it looks okay to keep balance of the platform and could - # allow us to move the basis as the VR pybullet example. + # allow us to move the base as the VR pybullet example. self.base_cid = None + self.in_range = None + print(self.base_cid) if not use_fixed_base: self.base_cid = sim.createConstraint( self.info.robot_id, -1, -1, -1, sim.JOINT_FIXED, [0.0, 0, 0], [0.0, 0, 0], base_pos) + print(self.base_cid) # Reset to initial position and visualize. self.rest_qpos = (self.info.joint_maxpos+self.info.joint_minpos)/2 if rest_arm_qpos is not None: + # print(len(self.info.arm_jids_lst)) assert(len(self.info.arm_jids_lst)==len(rest_arm_qpos)) self.rest_qpos[self.info.arm_jids_lst] = rest_arm_qpos[:] if left_rest_arm_qpos is not None: assert(len(self.info.left_arm_jids_lst)==len(left_rest_arm_qpos)) self.rest_qpos[self.info.left_arm_jids_lst] = left_rest_arm_qpos[:] + print('before reset: q_pos', self.rest_qpos) self.reset() ee_pos, ee_ori, *_ = self.get_ee_pos_ori_vel() - print('Initialized robot ee at pos', ee_pos, + + print('Initialized robot ee-r at pos', ee_pos, 'euler ori', sin_cos_to_theta(ee_ori), 'sin/cos ori', ee_ori) + print('Initial pose',self.get_qpos()) + if (left_ee_link_name is not None): + ee_pos_l, ee_ori_l, *_ = self.get_ee_pos_ori_vel(left = True) + print('Initialized robot ee-l at pos', ee_pos_l, + 'euler ori', sin_cos_to_theta(ee_ori_l), + 'sin/cos ori', ee_ori_l) def load_robot(self, robot_path, ee_joint_name, ee_link_name, left_ee_joint_name, left_ee_link_name, left_fing_link_prefix, left_joint_suffix, base_pos, base_quat, use_fixed_base, global_scaling): + robot_id = self.sim.loadURDF( robot_path, basePosition=base_pos, baseOrientation=base_quat, useFixedBase=use_fixed_base, flags=pybullet.URDF_USE_SELF_COLLISION, @@ -278,13 +291,19 @@ def _ee_pos_to_qpos_raw(self, ee_pos, ee_ori=None, fing_dist=0.0, ee_quat = None if ee_ori is not None: ee_quat = sin_cos_to_quat(ee_ori) + ################# USES NULL SPACE- IK INFLUENCED BY RESTPOSE ################ + # qpos = pybullet.calculateInverseKinematics( + # self.info.robot_id, self.info.ee_link_id, + # targetPosition=ee_pos.tolist(), targetOrientation=ee_quat, + # lowerLimits=self.info.joint_minpos.tolist(), + # upperLimits=self.info.joint_maxpos.tolist(), + # jointRanges=(self.info.joint_maxpos-self.info.joint_minpos).tolist(), + # restPoses= self.get_qpos().tolist(), # self.rest_qpos.tolist(), + # maxNumIterations=500, residualThreshold=0.005, solver=pybullet.IK_SDLS) + ################### DEFAULT IK IS USED #################################### qpos = pybullet.calculateInverseKinematics( self.info.robot_id, self.info.ee_link_id, targetPosition=ee_pos.tolist(), targetOrientation=ee_quat, - lowerLimits=self.info.joint_minpos.tolist(), - upperLimits=self.info.joint_maxpos.tolist(), - jointRanges=(self.info.joint_maxpos-self.info.joint_minpos).tolist(), - restPoses=self.rest_qpos.tolist(), maxNumIterations=500, residualThreshold=0.005) # solver=pybullet.IK_SDLS, # maxNumIterations=1000, residualThreshold=0.0001) @@ -323,6 +342,7 @@ def _ee_pos_to_qpos_raw(self, ee_pos, ee_ori=None, fing_dist=0.0, left_fing_dist/2.0, self.info.joint_minpos[jid], self.info.joint_maxpos[jid]) # IK will find solutions outside of joint limits, so clip. + ###################### TAKE NOTES ############################### # qpos = np.clip(qpos, self.info.joint_minpos, self.info.joint_maxpos) return qpos @@ -362,7 +382,7 @@ def move_with_qvel(self, tgt_qvel, mode, kp=None, kd=None): tgt_qpos = np.zeros_like(tgt_qvel) ok_tgt_qvel = self.get_ok_qvel(tgt_qvel) if ok_tgt_qvel is not None: - self.move_to_qposvel(tgt_qpos, ok_tgt_qvel, mode, kp, kd) + self.vel(tgt_qpos, ok_tgt_qvel, mode, kp, kd) else: self.move_to_qposvel( tgt_qpos, np.zeros_like(tgt_qvel), mode, kp, kd) @@ -554,33 +574,30 @@ def get_relative_pose(self, pos, quat=None): inv_trans, inv_rot, pos, quat_input) return np.array(local_pos), None if quat is None else np.array(local_quat) - - def move_base(self, lin_vel, rot_vel, dt=1./240): + + def move_base(self, tgt_pos, ds=1./240): # Change fixed constraint spec to move the base: assuming the mobile # base is omnidirectional, we can also animate differential drive by # having the wheel distance. + + ds /= 5 assert(self.base_cid is not None) base_state = self.sim.getLinkState( self.info.robot_id, 0, computeLinkVelocity=0) base_pos = base_state[0] base_quat = base_state[1] + next_base_ori = np.array(self.sim.getEulerFromQuaternion(base_quat)) # print(base_pos, base_quat) - # delta_quat = self.sim.getQuaternionFromEuler([0, 0, rot_vel*dt]) - # tar_base_pos, tar_base_ori = self.sim.multiplyTransforms(base_pos, - # base_quat, [lin_vel[0] * dt, lin_vel[1] * dt, 0], delta_quat) + rel_pos = tgt_pos - base_pos next_base_pos = np.add(np.array(base_pos), - np.array([lin_vel[0]*dt, - lin_vel[1]*dt, 0])).tolist() - next_base_ori = np.add(np.array( - self.sim.getEulerFromQuaternion(base_quat)), - np.array([0, 0, rot_vel*dt])) - next_base_ori = self.sim.getQuaternionFromEuler(next_base_ori.tolist()) - # User needs to specify dt since it is specified by the user so + np.array([rel_pos[0]*ds, + rel_pos[1]*ds, 0])).tolist() + + # User needs to specify ds since it is specified by the user so # better let user to track it. # print(next_base_pos, next_base_ori) - self.sim.changeConstraint(self.base_cid, next_base_pos, next_base_ori, - maxForce=1000) - return + self.sim.changeConstraint(self.base_cid, next_base_pos,next_base_ori,maxForce=1000) + return # # Utilities to convert between "sin,cos" Euler angle representation to diff --git a/dedo/utils/preset_info.py b/dedo/utils/preset_info.py index f2802b2..a73bcfc 100644 --- a/dedo/utils/preset_info.py +++ b/dedo/utils/preset_info.py @@ -12,21 +12,34 @@ 'cloth/apron_0.obj': { # HangGarment-v0, 600 steps 'waypoints': { 'a': [ - # [ x, y, z, seconds(time)] - # [2, 2, 10.5, 1], # waypoint 0 - [2, -0.5, 9.5, 1], - [2, -1, 8, 1], - [2, -1.2, 7.5, 1], - ], - 'b': [ - # [ x, y, z, seconds(time)] - # [-2, 2, 10.5, 1], # waypoint 0 - # [-2, -0.5, 9.5, 1], - # [-2, -1, 8, 1], - [-1.6, -0.5, 9.5, 1], - [-1.6, -1, 8, 1], - [-1.6, -1.2, 7.5, 1], + # [ x, y, z, seconds(time)] + # [1.8, 3.7, 10.5, 0.5], # waypoint 0 + # [2, -0.5, 9.5, 0.5], + # [2, -1, 8, 0.5], + # [2, -1.2, 7.5, 0.5], + # [5.7, 0, 6.5, 1], # waypoint 0 + # [6,3,6.5,1] + [0,0,9.5,1] + # [7, 0, 8.5, 1], + # [5, 0, 10.5, 1], + # [3, 0, 9.5, 1] ], + # 'b': [ + # # [ x, y, z, seconds(time)] + # # [-2, 2, 10.5, 1], # waypoint 0 + # # [-2, -0.5, 9.5, 1], + # # [-2, -1, 8, 1], + # # [-1.3, 4.2 , 10.6, 0.5], # waypoint 0 + # [-1.6, -0.5, 9.5, 0.5], + # [-1.6, -1, 8, 0.5], + # [-1.6, -1.2, 7.5, 0.5], + # # [5, 0.5, 8.5, 1] # waypoint 0 + # # [9, 0, 8, 2], + # # [7, 0, 7.5, 2], + # # [5, 0, 8, 2], + # # [2.5, 0, 7.5, 2], + # # [0, 0, 7, 2], + # ], }, }, 'cloth/shirt_0.obj': { # HangGarment-v5, 1500 steps diff --git a/dedo/utils/task_info.py b/dedo/utils/task_info.py index 0523eae..401ed68 100644 --- a/dedo/utils/task_info.py +++ b/dedo/utils/task_info.py @@ -127,6 +127,7 @@ }, 'goal_pos': [[0.00, 1.28, 9]], }, + ################ RELEVANT ################################ 'hangcloth': { 'entities': { 'urdf/hanger.urdf': { @@ -145,6 +146,7 @@ }, 'goal_pos': [[0, 0.00, 8.2]], }, + ############################################################# 'button': { 'entities': { 'urdf/torso.urdf': { @@ -606,15 +608,15 @@ }, 'cloth/apron_0.obj': { - 'deform_init_pos': [0, 5, 8], + 'deform_init_pos': [0, 5, 8], # [5,0,3.6], 'deform_init_ori': [np.pi / 2, 0, np.pi], 'deform_scale': 3, 'deform_elastic_stiffness': 50, 'deform_bending_stiffness': 1, 'deform_damping_stiffness': 0.01, 'deform_anchor_vertices': [ - [15], # 10, 12, 13, 14, 15], - [170], # 163, 165, 167, 168, 170], + [15], # 10, 12, 13, 14, 15], # 50 + [170], # 163, 165, 167, 168, 170], # 51 ], 'cam_viewmat': [8.8, -12.6, 314, -0.4, 0.6, 5.3], @@ -1330,7 +1332,7 @@ 'use_fixed_base': True, 'base_pos': np.array([5.0, 1.5, 0]), 'rest_arm_qpos': np.array( - [-0.7332, -0.0135, 0.1112, -0.718, 0.0978, 1.99, -0.5592]), + [-0.7332, -0.0135, 0.1112, -0.718, 0.0978, 1.99, -0.5592]), 'left_rest_arm_qpos': np.array( [0.7732, -0.0135, -0.0212, -0.68, 0.0978, 1.99, -0.5592]), }, @@ -1338,12 +1340,25 @@ 'file_name': 'franka/franka_small_fingers.urdf', 'ee_joint_name': 'panda_joint7', 'ee_link_name': 'panda_hand', + 'left_ee_joint_name': 'panda_joint7', + 'left_ee_link_name': 'panda_hand', 'global_scaling': 10.0, 'use_fixed_base': True, 'base_pos': np.array([5.0, 1.5, 0]), 'rest_arm_qpos': np.array( - # for [2.5, 1.5, 1.0] + # # for [2.5, 1.5, 1.0] [0.4083, 0.4691, -0.6216, -2.9606, -0.9926, 3.4903, 1.5129] + # # [0.7732, -0.0135, -0.0212, -0.68, 0.0978, 1.99, -0.5592] ), + }, + 'fetch': { + 'file_name': 'fetch/fetch.urdf', + 'ee_joint_name': 'wrist_roll_joint', + 'ee_link_name': 'gripper_link', + 'global_scaling': 10.0, + 'use_fixed_base': False, + 'base_pos': np.array([16.0, 0, 0]), + 'rest_arm_qpos': None + } } diff --git a/results/2_roll_unfixed.mp4 b/results/2_roll_unfixed.mp4 new file mode 100644 index 0000000..01677e7 Binary files /dev/null and b/results/2_roll_unfixed.mp4 differ diff --git a/results/all_roll_unfixed.mp4 b/results/all_roll_unfixed.mp4 new file mode 100644 index 0000000..a7c72e1 Binary files /dev/null and b/results/all_roll_unfixed.mp4 differ diff --git a/results/roll_joints_fixed.mp4 b/results/roll_joints_fixed.mp4 new file mode 100644 index 0000000..c44be81 Binary files /dev/null and b/results/roll_joints_fixed.mp4 differ diff --git a/results/wrist_roll_allowed.mp4 b/results/wrist_roll_allowed.mp4 new file mode 100644 index 0000000..59b1207 Binary files /dev/null and b/results/wrist_roll_allowed.mp4 differ