diff --git a/robohive/envs/arms/fetch/assets/fetch_reach_v0.config b/robohive/envs/arms/fetch/assets/fetch_reach_v0.config index b35c8ae3..aab9fd2f 100644 --- a/robohive/envs/arms/fetch/assets/fetch_reach_v0.config +++ b/robohive/envs/arms/fetch/assets/fetch_reach_v0.config @@ -3,27 +3,27 @@ 'franka':{ 'interface': {'type': 'fetch'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_pan_jp'}, - {'range':(-1.8, 1.8), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_lift_jp'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'upperarm_roll_jp'}, - {'range':(-3.1, 0.0), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'elbow_flex_jp'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'forearm_roll_jp'}, - {'range':(-1.7, 3.8), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'wrist_flex_jp'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'wrist_roll_jp'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_jp'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_jp'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_pan_jp'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_lift_jp'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'upperarm_roll_jp'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'elbow_flex_jp'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'forearm_roll_jp'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'wrist_flex_jp'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'wrist_roll_jp'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_jp'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_jp'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_pan'}, - {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_lift'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'upperarm_roll'}, - {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'elbow_flex'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'forearm_roll'}, - {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'wrist_flex'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'wrist_roll'}, - {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger'}, - {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_pan'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_lift'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'upperarm_roll'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'elbow_flex'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'forearm_roll'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'wrist_flex'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'wrist_roll'}, + {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger'}, + {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger'}, ] } } \ No newline at end of file diff --git a/robohive/envs/arms/franka/assets/franka_busbin_v0.config b/robohive/envs/arms/franka/assets/franka_busbin_v0.config index 2904463b..bf2894e8 100644 --- a/robohive/envs/arms/franka/assets/franka_busbin_v0.config +++ b/robohive/envs/arms/franka/assets/franka_busbin_v0.config @@ -3,39 +3,39 @@ 'franka':{ 'interface': {'type': 'franka', 'ip_address':'169.254.163.91'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, - {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, - {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, - {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, - {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, - {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, - {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, ] }, # 'busbin':{ # 'interface': {}, # 'sensor':[ - # {'range':(-2.0, 0.0), 'noise':0.05, 'adr':7, 'scale':1, 'offset':0, 'name':'???'}, + # {'range':(-2.0, 0.0), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'???'}, # ], # 'actuator':[] # } diff --git a/robohive/envs/arms/franka/assets/franka_reach_v0.config b/robohive/envs/arms/franka/assets/franka_reach_v0.config index 04f0819b..87b489cd 100644 --- a/robohive/envs/arms/franka/assets/franka_reach_v0.config +++ b/robohive/envs/arms/franka/assets/franka_reach_v0.config @@ -3,37 +3,37 @@ 'franka':{ 'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale':0.5}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, - {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, - {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, - {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, - # {'range':(0.00, .04), 'noise':0.05, 'adr':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, - # {'range':(0.00, .04), 'noise':0.05, 'adr':8, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + # {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, + # {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':8, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, - {'pos_range':(-1.8326, 1.8326), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, - {'pos_range':(-3.1416, 0.0000), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, - {'pos_range':(-1.6600, 2.1817), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, - # {'pos_range':(-0.0000, 0.0400), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'}, - # {'pos_range':(-0.0000, 0.0400), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, + # {'pos_range':(-0.0000, 0.0400), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'}, + # {'pos_range':(-0.0000, 0.0400), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'}, ] }, @@ -41,8 +41,8 @@ 'interface': {'type': 'realsense', 'topic':'realsense_815412070228/color/image_raw', 'data_type':'rgb240x320'}, 'sensor':[], 'cam': [ - {'range':(0, 255), 'noise':0.00, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, - # {'range':(0, 255), 'noise':0.00, 'adr':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + # {'range':(0, 255), 'noise':0.00, 'hdr_adr':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'}, ], 'actuator':[] }, @@ -51,8 +51,8 @@ 'interface': {'type': 'realsense', 'topic':'realsense_815412070341/color/image_raw', 'data_type':'rgb'}, 'sensor':[], 'cam': [ - {'range':(0, 255), 'noise':0.00, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, - # {'range':(0, 255), 'noise':0.00, 'adr':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + # {'range':(0, 255), 'noise':0.00, 'hdr_adr':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'}, ], 'actuator':[] }, @@ -61,8 +61,8 @@ 'interface': {'type': 'realsense', 'topic':'realsense_936322070233/color/image_raw', 'data_type':'rgb'}, 'sensor':[], 'cams': [ - {'range':(0, 255), 'noise':0.00, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, - # {'range':(0, 255), 'noise':0.00, 'adr':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + # {'range':(0, 255), 'noise':0.00, 'hdr_adr':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'}, ], 'actuator':[] }, diff --git a/robohive/envs/arms/franka/assets/franka_ycb_v0.config b/robohive/envs/arms/franka/assets/franka_ycb_v0.config index 81b49b42..e9a9297c 100644 --- a/robohive/envs/arms/franka/assets/franka_ycb_v0.config +++ b/robohive/envs/arms/franka/assets/franka_ycb_v0.config @@ -3,48 +3,48 @@ 'franka':{ 'interface': {'type': 'franka', 'ip_address':'169.254.163.91'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, - {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, - {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, - {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':8, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':8, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, - {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, - {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, - {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, - {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'}, - {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, + {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'}, + {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'}, ] }, 'object':{ 'interface':{}, 'sensor':[ - {'range':(-1.0, 1.0), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'Tx'}, - {'range':(-0.5, 0.5), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'Ty'}, - {'range':(-1.0, 1.0), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'Tz'}, - {'range':(-3.1, 3.1), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'Rx'}, - {'range':(-3.1, 3.1), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'Ry'}, - {'range':(-3.1, 3.1), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'Rz'}, + {'range':(-1.0, 1.0), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Tx'}, + {'range':(-0.5, 0.5), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Ty'}, + {'range':(-1.0, 1.0), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Tz'}, + {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Rx'}, + {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Ry'}, + {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Rz'}, ], 'actuator':[] } diff --git a/robohive/envs/env_base.py b/robohive/envs/env_base.py index cd7edd19..6e9afac2 100644 --- a/robohive/envs/env_base.py +++ b/robohive/envs/env_base.py @@ -74,6 +74,7 @@ def _setup(self, obs_range:tuple = (-10, 10), # Permissible range of values in obs vector returned by get_obs() rwd_viz:bool = False, # Visualize rewards (WIP, needs vtils) device_id:int = 0, # Device id for rendering + init_qpos = None, # Explicit reset/home qpos. Auto-computed (mid actuator range) if not provided **kwargs, # Additional arguments ): @@ -87,7 +88,8 @@ def _setup(self, self.viewer_setup() # resolve robot config - self.robot = Robot(mj_sim=self.sim, + robot_cls = kwargs.pop('robot_cls', Robot) + self.robot = robot_cls(mj_sim=self.sim, random_generator=self.np_random, **kwargs) @@ -100,18 +102,25 @@ def _setup(self, # resolve initial state self.init_qvel = self.sim.data.qvel.ravel().copy() - self.init_qpos = self.sim.data.qpos.ravel().copy() # has issues with initial jump during reset - # self.init_qpos = np.mean(self.sim.model.actuator_ctrlrange, axis=1) if self.normalize_act else self.sim.data.qpos.ravel().copy() # has issues when nq!=nu - # self.init_qpos[self.sim.model.jnt_dofadr] = np.mean(self.sim.model.jnt_range, axis=1) if self.normalize_act else self.sim.data.qpos.ravel().copy() - if self.normalize_act: - # find all linear+actuated joints. Use mean(jnt_range) as init position - actuated_jnt_ids = self.sim.model.actuator_trnid[self.sim.model.actuator_trntype==self.sim.lib.mjtTrn.mjTRN_JOINT, 0] # dm - linear_jnt_ids = np.logical_or(self.sim.model.jnt_type==self.sim.lib.mjtJoint.mjJNT_SLIDE, self.sim.model.jnt_type==self.sim.lib.mjtJoint.mjJNT_HINGE) - linear_jnt_ids = np.where(linear_jnt_ids==True)[0] - linear_actuated_jnt_ids = np.intersect1d(actuated_jnt_ids, linear_jnt_ids) - # assert np.any(actuated_jnt_ids==linear_actuated_jnt_ids), "Wooho: Great evidence that it was important to check for actuated_jnt_ids as well as linear_actuated_jnt_ids" - linear_actuated_jnt_qposids = self.sim.model.jnt_qposadr[linear_actuated_jnt_ids] - self.init_qpos[linear_actuated_jnt_qposids] = np.mean(self.sim.model.jnt_range[linear_actuated_jnt_ids], axis=1) + if init_qpos is not None: + # use the provided init_pos + self.init_qpos = np.array(init_qpos, dtype=np.float64).ravel().copy() + else: + # create one if not provided + self.init_qpos = self.sim.data.qpos.ravel().copy() # has issues with initial jump during reset + if self.normalize_act: + # find all linear+actuated joints. Use mean(jnt_range) as init position + actuated_jnt_ids = self.sim.model.actuator_trnid[self.sim.model.actuator_trntype==self.sim.lib.mjtTrn.mjTRN_JOINT, 0] # dm + linear_jnt_ids = np.logical_or(self.sim.model.jnt_type==self.sim.lib.mjtJoint.mjJNT_SLIDE, self.sim.model.jnt_type==self.sim.lib.mjtJoint.mjJNT_HINGE) + linear_jnt_ids = np.where(linear_jnt_ids==True)[0] + linear_actuated_jnt_ids = np.intersect1d(actuated_jnt_ids, linear_jnt_ids) + linear_actuated_jnt_qposids = self.sim.model.jnt_qposadr[linear_actuated_jnt_ids] + self.init_qpos[linear_actuated_jnt_qposids] = np.mean(self.sim.model.jnt_range[linear_actuated_jnt_ids], axis=1) + if self.robot.is_hardware: + prompt(f"WARNING: {self.robot.name} is hardware-backed but no init_qpos was provided — " + "defaulting to the mid-range actuator pose. Pass init_qpos explicitly " + "to control where the robot homes to on reset.", + type=Prompt.WARN) # resolve rewards self.rwd_dict = {} @@ -131,10 +140,11 @@ def _setup(self, self.visual_keys = visual_keys if type(visual_keys)==list or visual_keys==None else [visual_keys] self._setup_rgb_encoders(self.visual_keys, device=None) - # reset to get the env ready - observation, _reward, done, *_, _info = self.step(np.zeros(self.sim.model.nu)) - # Question: Should we replace above with following? Its specially helpful for hardware as it forces a env reset before continuing, without which the hardware will make a big jump from its position to the position asked by step. - # observation = self.reset() + # reset to get the env ready. Using reset() rather than step(zeros) routes hardware + # through Robot.reset()'s min-jerk hardware_reset(), instead of jumping straight to + # a raw ctrl command from whatever pose the robot is currently in. + self.reset() + observation, _reward, done, *_, _info = self.forward() assert not done, "Check initialization. Simulation starts in a done state." self.observation_space = gym.spaces.Box(obs_range[0]*np.ones(observation.size), obs_range[1]*np.ones(observation.size), dtype=np.float32) diff --git a/robohive/envs/fm/assets/dmanus.config b/robohive/envs/fm/assets/dmanus.config index a0bf3062..7a307820 100644 --- a/robohive/envs/fm/assets/dmanus.config +++ b/robohive/envs/fm/assets/dmanus.config @@ -1,29 +1,29 @@ { # device1: sensors, actuators 'dmanus':{ - 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/ttyUSB0"}, + 'interface': {'type': 'dynamixel', 'motor_type':"X", 'port':"/dev/ttyUSB0"}, 'sensor':[ - {'range':(-0.75, 0.57), 'noise':0.05, 'adr':10, 'name':'TFJ1', 'scale':-1, 'offset':np.pi }, - {'range':(-0.00, 2.14), 'noise':0.05, 'adr':11, 'name':'TFJ2', 'scale':-1, 'offset':3*np.pi/2 }, - {'range':(-0.00, 2.00), 'noise':0.05, 'adr':12, 'name':'TFJ3', 'scale':-1, 'offset':np.pi }, - {'range':(-0.75, 0.57), 'noise':0.05, 'adr':20, 'name':'IFJ1', 'scale':-1, 'offset':np.pi }, - {'range':(-0.00, 2.14), 'noise':0.05, 'adr':21, 'name':'IFJ2', 'scale':-1, 'offset':3*np.pi/2 }, - {'range':(-0.00, 2.00), 'noise':0.05, 'adr':22, 'name':'IFJ3', 'scale':+1, 'offset':-np.pi }, - {'range':(-0.75, 0.57), 'noise':0.05, 'adr':30, 'name':'LFJ1', 'scale':-1, 'offset':np.pi }, - {'range':(-0.00, 2.14), 'noise':0.05, 'adr':31, 'name':'LFJ2', 'scale':+1, 'offset':-np.pi/2 }, - {'range':(-0.00, 2.00), 'noise':0.05, 'adr':32, 'name':'LFJ3', 'scale':+1, 'offset':-np.pi }, + {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':10, 'name':'TFJ1', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':11, 'name':'TFJ2', 'scale':-1, 'offset':3*np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':12, 'name':'TFJ3', 'scale':-1, 'offset':np.pi }, + {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':20, 'name':'IFJ1', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':21, 'name':'IFJ2', 'scale':-1, 'offset':3*np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':22, 'name':'IFJ3', 'scale':+1, 'offset':-np.pi }, + {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':30, 'name':'LFJ1', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':31, 'name':'LFJ2', 'scale':+1, 'offset':-np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':32, 'name':'LFJ3', 'scale':+1, 'offset':-np.pi }, ], 'actuator':[ - {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':10, 'name':'TFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi }, - {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':11, 'name':'TFA2', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 }, - {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':12, 'name':'TFA3', 'mode':'Position', 'scale':-1, 'offset':np.pi }, - {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':20, 'name':'IFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi }, - {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':21, 'name':'IFA2', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 }, - {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':22, 'name':'IFA3', 'mode':'Position', 'scale':+1, 'offset':np.pi }, - {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':30, 'name':'LFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi }, - {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':31, 'name':'LFA2', 'mode':'Position', 'scale':+1, 'offset':np.pi/2 }, - {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':32, 'name':'LFA3', 'mode':'Position', 'scale':+1, 'offset':np.pi }, + {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'name':'TFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'name':'TFA2', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 }, + {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'name':'TFA3', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'name':'IFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'name':'IFA2', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 }, + {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'name':'IFA3', 'mode':'Position', 'scale':+1, 'offset':np.pi }, + {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'name':'LFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'name':'LFA2', 'mode':'Position', 'scale':+1, 'offset':np.pi/2 }, + {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'name':'LFA3', 'mode':'Position', 'scale':+1, 'offset':np.pi }, ] } } \ No newline at end of file diff --git a/robohive/envs/fm/assets/franka_dmanus.config b/robohive/envs/fm/assets/franka_dmanus.config index 2384ee5b..478e3d1e 100644 --- a/robohive/envs/fm/assets/franka_dmanus.config +++ b/robohive/envs/fm/assets/franka_dmanus.config @@ -5,53 +5,53 @@ 'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale':0.2}, # 'interface': {'type': 'franka', 'ip_address':'169.254.163.91'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, - {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, - {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, - {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, - {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, - {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, - {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, ] }, # device1: sensors, actuators 'dmanus':{ - 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/ttyUSB0"}, + 'interface': {'type': 'dynamixel', 'motor_type':"X", 'port':"/dev/ttyUSB0"}, 'sensor':[ - {'range':(-0.75, 0.57), 'noise':0.05, 'adr':10, 'name':'TF_ADB_jp', 'scale':-1, 'offset':np.pi }, - {'range':(-0.00, 2.14), 'noise':0.05, 'adr':11, 'name':'TF_MCP_jp', 'scale':-1, 'offset':3*np.pi/2 }, - {'range':(-0.00, 2.14), 'noise':0.05, 'adr':12, 'name':'TF_PIP_jp', 'scale':-1, 'offset':np.pi }, - {'range':(-0.00, 2.00), 'noise':0.05, 'adr':13, 'name':'TF_DIP_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':10, 'name':'TF_ADB_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':11, 'name':'TF_MCP_jp', 'scale':-1, 'offset':3*np.pi/2 }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':12, 'name':'TF_PIP_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':13, 'name':'TF_DIP_jp', 'scale':-1, 'offset':np.pi }, - {'range':(-0.75, 0.57), 'noise':0.05, 'adr':20, 'name':'FF_ADB_jp', 'scale':-1, 'offset':np.pi }, - {'range':(-0.00, 2.14), 'noise':0.05, 'adr':21, 'name':'FF_MCP_jp', 'scale':-1, 'offset':3*np.pi/2 }, - {'range':(-0.00, 2.00), 'noise':0.05, 'adr':22, 'name':'FF_PIP_jp', 'scale':+1, 'offset':-np.pi }, - {'range':(-0.75, 0.57), 'noise':0.05, 'adr':30, 'name':'PF_ADB_jp', 'scale':-1, 'offset':np.pi }, - {'range':(-0.00, 2.14), 'noise':0.05, 'adr':31, 'name':'PF_MCP_jp', 'scale':+1, 'offset':-np.pi/2 }, - {'range':(-0.00, 2.00), 'noise':0.05, 'adr':32, 'name':'PF_PIP_jp', 'scale':+1, 'offset':-np.pi }, + {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':20, 'name':'FF_ADB_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':21, 'name':'FF_MCP_jp', 'scale':-1, 'offset':3*np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':22, 'name':'FF_PIP_jp', 'scale':+1, 'offset':-np.pi }, + {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':30, 'name':'PF_ADB_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':31, 'name':'PF_MCP_jp', 'scale':+1, 'offset':-np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':32, 'name':'PF_PIP_jp', 'scale':+1, 'offset':-np.pi }, ], 'actuator':[ - {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':10, 'name':'TF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi }, - {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':11, 'name':'TF_MCP', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 }, - {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':12, 'name':'TF_PIP', 'mode':'Position', 'scale':-1, 'offset':np.pi }, - {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':13, 'name':'TF_DIP', 'mode':'Position', 'scale':-1, 'offset':np.pi }, - {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':20, 'name':'FF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi }, - {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':21, 'name':'FF_MCP', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 }, - {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':22, 'name':'FF_PIP', 'mode':'Position', 'scale':+1, 'offset':np.pi }, - {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':30, 'name':'PF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi }, - {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':31, 'name':'PF_MCP', 'mode':'Position', 'scale':+1, 'offset':np.pi/2 }, - {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':32, 'name':'PF_PIP', 'mode':'Position', 'scale':+1, 'offset':np.pi }, + {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'name':'TF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'name':'TF_MCP', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'name':'TF_PIP', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':13, 'name':'TF_DIP', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'name':'FF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'name':'FF_MCP', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 }, + {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'name':'FF_PIP', 'mode':'Position', 'scale':+1, 'offset':np.pi }, + {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'name':'PF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'name':'PF_MCP', 'mode':'Position', 'scale':+1, 'offset':np.pi/2 }, + {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'name':'PF_PIP', 'mode':'Position', 'scale':+1, 'offset':np.pi }, ] } } \ No newline at end of file diff --git a/robohive/envs/fm/assets/franka_robotiq.config b/robohive/envs/fm/assets/franka_robotiq.config index 7ed26bb2..73d68220 100644 --- a/robohive/envs/fm/assets/franka_robotiq.config +++ b/robohive/envs/fm/assets/franka_robotiq.config @@ -4,23 +4,23 @@ 'franka':{ 'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale':0.5}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, - {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, - {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, - {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, - {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, - {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, - {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, ] }, @@ -28,10 +28,10 @@ 'robotiq':{ 'interface': {'type': 'robotiq', 'ip_address':'172.16.0.1'}, 'sensor':[ - {'range':(0, 0.834), 'noise':0.0, 'adr':0, 'name':'robotiq_2f_85', 'scale':-9.81, 'offset':0.834}, + {'range':(0, 0.834), 'noise':0.0, 'hdr_adr':0, 'name':'robotiq_2f_85', 'scale':-9.81, 'offset':0.834}, ], 'actuator':[ - {'pos_range':(0, 1), 'vel_range':(-20*np.pi/4, 20*np.pi/4), 'adr':0, 'name':'robotiq_2f_85', 'scale':-0.085, 'offset':0.085}, + {'pos_range':(0, 1), 'vel_range':(-20*np.pi/4, 20*np.pi/4), 'hdr_adr':0, 'name':'robotiq_2f_85', 'scale':-0.085, 'offset':0.085}, ] }, @@ -39,8 +39,8 @@ 'interface': {'type': 'realsense', 'rgb_topic':'realsense_815412070228/color/image_raw', 'd_topic':'realsense_815412070228/depth_uncolored/image_raw'}, 'sensor':[], 'cam': [ - {'range':(0, 255), 'noise':0.00, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, - {'range':(0, 255), 'noise':0.00, 'adr':'d', 'scale':1, 'offset':0, 'name':'/depth_uncolored/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'d', 'scale':1, 'offset':0, 'name':'/depth_uncolored/image_raw'}, ], 'actuator':[] }, @@ -49,7 +49,7 @@ 'interface': {'type': 'realsense', 'rgb_topic':'realsense_815412070341/color/image_raw', 'd_topic':'realsense_815412070341/depth_uncolored/image_raw'}, 'sensor':[], 'cam': [ - {'range':(0, 255), 'noise':0.00, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, ], 'actuator':[] }, @@ -58,7 +58,7 @@ 'interface': {'type': 'realsense', 'rgb_topic':'realsense_936322070233/color/image_raw', 'd_topic':'realsense_936322070233/depth_uncolored/image_raw'}, 'sensor':[], 'cam': [ - {'range':(0, 255), 'noise':0.00, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, ], 'actuator':[] }, @@ -67,7 +67,7 @@ 'interface': {'type': 'realsense', 'rgb_topic':'realsense_814412070228/color/image_raw', 'd_topic':'realsense_814412070228/depth_uncolored/image_raw'}, 'sensor':[], 'cam': [ - {'range':(0, 255), 'noise':0.00, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, ], 'actuator':[] }, diff --git a/robohive/envs/hands/baoding_v1.py b/robohive/envs/hands/baoding_v1.py index 1b6d97da..fcf35517 100644 --- a/robohive/envs/hands/baoding_v1.py +++ b/robohive/envs/hands/baoding_v1.py @@ -131,11 +131,9 @@ def _setup(self, super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, frame_skip=frame_skip, + init_qpos=self.sim.model.key_qpos[0].copy(), **kwargs, ) - - # reset position - self.init_qpos = self.sim.model.key_qpos[0].copy() # self.init_qpos[:-14] *= 0 # Use fully open as init pos # V0: Centered the action space around key_qpos[0]. Not sure if it matter. diff --git a/robohive/envs/hands/door_v1.py b/robohive/envs/hands/door_v1.py index 4e756e9f..18dbfdc3 100644 --- a/robohive/envs/hands/door_v1.py +++ b/robohive/envs/hands/door_v1.py @@ -59,8 +59,8 @@ def _setup(self, weighted_reward_keys=weighted_reward_keys, reward_mode=reward_mode, frame_skip=frame_skip, + init_qpos=np.zeros(sim.data.qpos.shape), **kwargs) - self.init_qpos = np.zeros(self.init_qpos.shape) self.init_qvel = np.zeros(self.init_qpos.shape) diff --git a/robohive/envs/hands/hammer_v1.py b/robohive/envs/hands/hammer_v1.py index cbdccb92..dfd4ecb0 100644 --- a/robohive/envs/hands/hammer_v1.py +++ b/robohive/envs/hands/hammer_v1.py @@ -66,8 +66,8 @@ def _setup(self, weighted_reward_keys=weighted_reward_keys, reward_mode=reward_mode, frame_skip=frame_skip, + init_qpos=np.zeros(sim.data.qpos.shape), **kwargs) - self.init_qpos = np.zeros(self.init_qpos.shape) self.init_qvel = np.zeros(self.init_qpos.shape) diff --git a/robohive/envs/hands/pen_v1.py b/robohive/envs/hands/pen_v1.py index f38c1c1d..0ca38e9c 100644 --- a/robohive/envs/hands/pen_v1.py +++ b/robohive/envs/hands/pen_v1.py @@ -65,8 +65,8 @@ def _setup(self, weighted_reward_keys=weighted_reward_keys, reward_mode=reward_mode, frame_skip=frame_skip, + init_qpos=np.zeros(sim.data.qpos.shape), **kwargs) - self.init_qpos = np.zeros(self.init_qpos.shape) self.init_qvel = np.zeros(self.init_qpos.shape) diff --git a/robohive/envs/hands/relocate_v1.py b/robohive/envs/hands/relocate_v1.py index be2d8ea0..003d9df0 100644 --- a/robohive/envs/hands/relocate_v1.py +++ b/robohive/envs/hands/relocate_v1.py @@ -64,8 +64,8 @@ def _setup(self, weighted_reward_keys=weighted_reward_keys, reward_mode=reward_mode, frame_skip=frame_skip, + init_qpos=np.zeros(sim.data.qpos.shape), **kwargs) - self.init_qpos = np.zeros(self.init_qpos.shape) self.init_qvel = np.zeros(self.init_qpos.shape) diff --git a/robohive/envs/multi_task/common/kitchen/franka_kitchen.config b/robohive/envs/multi_task/common/kitchen/franka_kitchen.config index 1fa983b7..c98ab660 100644 --- a/robohive/envs/multi_task/common/kitchen/franka_kitchen.config +++ b/robohive/envs/multi_task/common/kitchen/franka_kitchen.config @@ -3,38 +3,38 @@ 'franka':{ 'interface': {'type': 'franka'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, - {'range':(-1.8, 1.8), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, - {'range':(-3.1, 0.0), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, - {'range':(-1.7, 3.8), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp6'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp7'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp7'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, ], 'actuator':[ # TODO: ranges here exceed the corresponding sensor ranges - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, - {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, - {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, - {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint6'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint7'}, - {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'}, - {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint7'}, + {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'}, + {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'}, ] }, @@ -42,22 +42,22 @@ 'kitchen':{ 'interface': {'type': 'tbd'}, 'sensor':[ - {'range':(-1.57, 00), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'knob1_joint'}, - {'range':(-1.57, 00), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'knob2_joint'}, - {'range':(-1.57, 00), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'knob3_joint'}, - {'range':(-1.57, 00), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'knob4_joint'}, - {'range':(-0.7, 0.0), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'lightswitch_joint'}, - {'range':(0.0, 1.57), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'slidedoor_joint'}, - {'range':(-1.57, .0), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'leftdoorhinge'}, - {'range':(0.0, 1.57), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'rightdoorhinge'}, - {'range':(-2.09, 00), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'micro0joint'}, + {'range':(-1.57, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'knob1_joint'}, + {'range':(-1.57, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'knob2_joint'}, + {'range':(-1.57, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'knob3_joint'}, + {'range':(-1.57, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'knob4_joint'}, + {'range':(-0.7, 0.0), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'lightswitch_joint'}, + {'range':(0.0, 1.57), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'slidedoor_joint'}, + {'range':(-1.57, .0), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'leftdoorhinge'}, + {'range':(0.0, 1.57), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'rightdoorhinge'}, + {'range':(-2.09, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'micro0joint'}, - {'range':(-1.25, 1.75), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Tx'}, - {'range':(-1.50, 1.50), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Ty'}, - {'range':(-0.10, 2.90), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Tz'}, - {'range':(-3.14, 3.14), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Rx'}, - {'range':(-3.14, 3.14), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Ry'}, - {'range':(-3.14, 3.14), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Rz'}, + {'range':(-1.25, 1.75), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Tx'}, + {'range':(-1.50, 1.50), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Ty'}, + {'range':(-0.10, 2.90), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Tz'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Rx'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Ry'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Rz'}, ], 'actuator':[] diff --git a/robohive/envs/multi_task/common/microwave/franka_microwave.config b/robohive/envs/multi_task/common/microwave/franka_microwave.config index 0d58447b..233094c0 100644 --- a/robohive/envs/multi_task/common/microwave/franka_microwave.config +++ b/robohive/envs/multi_task/common/microwave/franka_microwave.config @@ -3,39 +3,39 @@ 'franka':{ 'interface': {'type': 'franka', 'ip_address':'169.254.163.91'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, - {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, - {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, - {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, - {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, - {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, - {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, ] }, 'microwave':{ 'interface': {}, 'sensor':[ - {'range':(-2.0, 0.0), 'noise':0.05, 'adr':7, 'scale':1, 'offset':0, 'name':'micro0joint'}, + {'range':(-2.0, 0.0), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'micro0joint'}, ], 'actuator':[] } diff --git a/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.config b/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.config index d7023e1f..8c518054 100644 --- a/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.config +++ b/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.config @@ -3,41 +3,41 @@ 'franka':{ 'interface': {'type': 'franka', 'ip_address':'169.254.163.91'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, - {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, - {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, - {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, - {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, - {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, - {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, - {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, - {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, ] }, 'slidecabinet':{ 'interface': {}, 'sensor':[ - {'range':(-0.0, .44), 'noise':0.05, 'adr':7, 'scale':1, 'offset':0, 'name':'slidedoor_joint'}, + {'range':(-0.0, .44), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'slidedoor_joint'}, ], 'actuator':[] } diff --git a/robohive/envs/multi_task/multi_task_base_v1.py b/robohive/envs/multi_task/multi_task_base_v1.py index 4122593e..50cb58b3 100644 --- a/robohive/envs/multi_task/multi_task_base_v1.py +++ b/robohive/envs/multi_task/multi_task_base_v1.py @@ -112,6 +112,13 @@ def _setup(self, self.input_obj_init = obj_init self.set_obj_goal(obj_goal=self.input_obj_goal, interact_site=interact_site) + # Recover init from the saved qposes and input specs before super()._setup(), since it + # triggers the first reset() which relies on init_qpos already reflecting these values. + keyFrame_id = 0 + self.init_qpos = self.sim.model.key_qpos[keyFrame_id].copy() + if obj_init: + self.set_obj_init(self.input_obj_init) + super()._setup(obs_keys=obs_keys_wt, proprio_keys=proprio_keys_wt, weighted_reward_keys=weighted_reward_keys, @@ -119,15 +126,9 @@ def _setup(self, act_mode=act_mode, obs_range=obs_range, robot_name=robot_name, + init_qpos=self.init_qpos, **kwargs) - - - # Recover init from the saved qposes and input specs - keyFrame_id = 0 - self.init_qpos[:] = self.sim.model.key_qpos[keyFrame_id].copy() self.init_qvel[:] = self.sim.model.key_qvel[keyFrame_id].copy() - if obj_init: - self.set_obj_init(self.input_obj_init) def get_dof_proximity(self, obj_dof_ranges, obj_dof_type): diff --git a/robohive/envs/myo/myobase/baoding_v1.py b/robohive/envs/myo/myobase/baoding_v1.py index 44f63fad..c5614f96 100644 --- a/robohive/envs/myo/myobase/baoding_v1.py +++ b/robohive/envs/myo/myobase/baoding_v1.py @@ -113,17 +113,19 @@ def _setup(self, # update rewards to be move rewards weighted_reward_keys = self.MOVE_TO_LOCATION_RWD_KEYS_AND_WEIGHTS + # reset position + # init_qpos = self.sim.model.key_qpos[0].copy() + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-14] *= 0 # Use fully open as init pos + init_qpos[0] = -1.57 # Palm up + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, frame_skip=frame_skip, + init_qpos=init_qpos, **kwargs, ) - # reset position - # self.init_qpos = self.sim.model.key_qpos[0].copy() - self.init_qpos[:-14] *= 0 # Use fully open as init pos - self.init_qpos[0] = -1.57 # Palm up - # V0: Centered the action space around key_qpos[0]. Not sure if it matter. # self.act_mid = self.init_qpos[:self.n_jnt].copy() # self.upper_rng = 0.9*(self.model.actuator_ctrlrange[:,1]-self.act_mid) diff --git a/robohive/envs/myo/myobase/key_turn_v0.py b/robohive/envs/myo/myobase/key_turn_v0.py index 9fc51e1d..e4f3657b 100644 --- a/robohive/envs/myo/myobase/key_turn_v0.py +++ b/robohive/envs/myo/myobase/key_turn_v0.py @@ -54,11 +54,14 @@ def _setup(self, self.key_init_range = key_init_range self.key_init_pos = self.sim.data.site_xpos[self.keyhead_sid].copy() + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-1] *= 0 # Use fully open as init pos + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=init_qpos, **kwargs, ) - self.init_qpos[:-1] *= 0 # Use fully open as init pos def get_obs_vec(self): self.obs_dict['time'] = np.array([self.sim.data.time]) diff --git a/robohive/envs/myo/myobase/obj_hold_v0.py b/robohive/envs/myo/myobase/obj_hold_v0.py index b0864933..bfc29c0d 100644 --- a/robohive/envs/myo/myobase/obj_hold_v0.py +++ b/robohive/envs/myo/myobase/obj_hold_v0.py @@ -47,12 +47,15 @@ def _setup(self, self.goal_sid = self.sim.model.site_name2id("goal") self.object_init_pos = self.sim.data.site_xpos[self.object_sid].copy() + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-7] *= 0 # Use fully open as init pos + init_qpos[0] = -1.5 # place palm up + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=init_qpos, **kwargs, ) - self.init_qpos[:-7] *= 0 # Use fully open as init pos - self.init_qpos[0] = -1.5 # place palm up def get_obs_vec(self): diff --git a/robohive/envs/myo/myobase/pen_v0.py b/robohive/envs/myo/myobase/pen_v0.py index 4684ed77..eb098524 100644 --- a/robohive/envs/myo/myobase/pen_v0.py +++ b/robohive/envs/myo/myobase/pen_v0.py @@ -57,12 +57,15 @@ def _setup(self, self.pen_length = np.linalg.norm(self.sim.model.site_pos[self.obj_t_sid] - self.sim.model.site_pos[self.obj_b_sid]) self.tar_length = np.linalg.norm(self.sim.model.site_pos[self.tar_t_sid] - self.sim.model.site_pos[self.tar_b_sid]) + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-6] *= 0 # Use fully open as init pos + init_qpos[0] = -1.5 # place palm up + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=init_qpos, **kwargs, ) - self.init_qpos[:-6] *= 0 # Use fully open as init pos - self.init_qpos[0] = -1.5 # place palm up def get_obs_vec(self): # qpos for hand, xpos for obj, xpos for target diff --git a/robohive/envs/myo/myobase/reorient_sar_v0.py b/robohive/envs/myo/myobase/reorient_sar_v0.py index ed99a603..246de692 100644 --- a/robohive/envs/myo/myobase/reorient_sar_v0.py +++ b/robohive/envs/myo/myobase/reorient_sar_v0.py @@ -69,13 +69,17 @@ def _setup(self, self.tar_length = np.linalg.norm(self.sim.model.geom_pos[self.tar_t_gid] - self.sim.model.geom_pos[self.tar_b_gid]) self.sim.model.body_mass[self.obj_bid] *= 1.25 + + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-6] *= 0 # Use fully open as init pos + init_qpos[0] = -1.5 # place palm up + super()._setup(obs_keys=['hand_jnt','obj_pos','obj_vel','obj_rot','obj_des_rot', 'obj_err_pos','obj_err_rot','mlen','mvel','mforce'], weighted_reward_keys=weighted_reward_keys, + init_qpos=init_qpos, **kwargs, ) - self.init_qpos[:-6] *= 0 # Use fully open as init pos - self.init_qpos[0] = -1.5 # place palm up def get_obs_dict(self, sim): diff --git a/robohive/envs/myo/myobase/walk_v0.py b/robohive/envs/myo/myobase/walk_v0.py index a5bf3164..65025cbd 100644 --- a/robohive/envs/myo/myobase/walk_v0.py +++ b/robohive/envs/myo/myobase/walk_v0.py @@ -53,9 +53,9 @@ def _setup(self, super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, sites=self.target_reach_range.keys(), + init_qpos=self.sim.model.key_qpos[0].copy(), **kwargs, ) - self.init_qpos[:] = self.sim.model.key_qpos[0] self.init_qvel[:] = self.sim.model.key_qvel[0] # find geometries with ID == 1 which indicates the skins geom_1_indices = np.where(self.sim.model.geom_group == 1) @@ -206,9 +206,9 @@ def _setup(self, self.steps = 0 super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=self.sim.model.key_qpos[0].copy(), **kwargs ) - self.init_qpos[:] = self.sim.model.key_qpos[0] self.init_qvel[:] = 0.0 # move heightfield down if not used @@ -473,9 +473,9 @@ def _setup(self, BaseV0._setup(self, obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=self.sim.model.key_qpos[0].copy(), **kwargs ) - self.init_qpos[:] = self.sim.model.key_qpos[0] self.init_qvel[:] = 0.0 def reset(self, **kwargs): diff --git a/robohive/envs/myo/myochallenge/baoding_v1.py b/robohive/envs/myo/myochallenge/baoding_v1.py index 861a1f40..c226b80a 100644 --- a/robohive/envs/myo/myochallenge/baoding_v1.py +++ b/robohive/envs/myo/myochallenge/baoding_v1.py @@ -96,16 +96,18 @@ def _setup(self, self.obj_friction_range = {'low':self.sim.model.geom_friction[self.object1_gid] - obj_friction_change, 'high':self.sim.model.geom_friction[self.object1_gid] + obj_friction_change} if obj_friction_change else None + # reset position + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-14] *= 0 # Use fully open as init pos + init_qpos[0] = -1.57 # Palm up + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, frame_skip=frame_skip, + init_qpos=init_qpos, **kwargs, ) - # reset position - self.init_qpos[:-14] *= 0 # Use fully open as init pos - self.init_qpos[0] = -1.57 # Palm up - def step(self, a, **kwargs): if self.which_task in [Task.HOLD, Task.BAODING_CW, Task.BAODING_CCW]: desired_angle_wrt_palm = self.goal[self.counter].copy() diff --git a/robohive/envs/myo/myochallenge/chasetag_v0.py b/robohive/envs/myo/myochallenge/chasetag_v0.py index 2c035765..09a6e475 100644 --- a/robohive/envs/myo/myochallenge/chasetag_v0.py +++ b/robohive/envs/myo/myochallenge/chasetag_v0.py @@ -679,7 +679,6 @@ def _setup(self, reset_type=reset_type, **kwargs ) - self.init_qpos[:] = self.sim.model.key_qpos[0] self.init_qvel[:] = 0.0 self.startFlag = True self.assert_settings() diff --git a/robohive/envs/myo/myochallenge/relocate_v0.py b/robohive/envs/myo/myochallenge/relocate_v0.py index 5547c169..36361e81 100644 --- a/robohive/envs/myo/myochallenge/relocate_v0.py +++ b/robohive/envs/myo/myochallenge/relocate_v0.py @@ -4,11 +4,13 @@ ================================================= """ import collections + import numpy as np -from robohive.utils import gym from robohive.envs.myo.base_v0 import BaseV0 -from robohive.utils.quat_math import mat2euler, euler2quat +from robohive.utils import gym +from robohive.utils.quat_math import euler2quat, mat2euler + class RelocateEnvV0(BaseV0): @@ -57,12 +59,12 @@ def _setup(self, self.rot_th = rot_th self.drop_th = drop_th + keyFrame_id = 0 if self.obj_xyz_range is None else 1 super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=self.sim.model.key_qpos[keyFrame_id].copy(), **kwargs, ) - keyFrame_id = 0 if self.obj_xyz_range is None else 1 - self.init_qpos[:] = self.sim.model.key_qpos[keyFrame_id].copy() def get_obs_dict(self, sim): diff --git a/robohive/envs/myo/myochallenge/reorient_v0.py b/robohive/envs/myo/myochallenge/reorient_v0.py index fd919193..92937f64 100644 --- a/robohive/envs/myo/myochallenge/reorient_v0.py +++ b/robohive/envs/myo/myochallenge/reorient_v0.py @@ -67,12 +67,15 @@ def _setup(self, self.obj_friction_range = {'low':self.sim.model.geom_friction[self.object_gid0:self.object_gidn] - obj_friction_change, 'high':self.sim.model.geom_friction[self.object_gid0:self.object_gidn] + obj_friction_change} + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-7] *= 0 # Use fully open as init pos + init_qpos[0] = -1.5 # Palm up + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=init_qpos, **kwargs, ) - self.init_qpos[:-7] *= 0 # Use fully open as init pos - self.init_qpos[0] = -1.5 # Palm up def get_obs_dict(self, sim): obs_dict = {} diff --git a/robohive/envs/myo/myodm/myodm_v0.py b/robohive/envs/myo/myodm/myodm_v0.py index 4c9ebeeb..012daab7 100644 --- a/robohive/envs/myo/myodm/myodm_v0.py +++ b/robohive/envs/myo/myodm/myodm_v0.py @@ -135,23 +135,25 @@ def _setup(self, self._lift_z = self.sim.data.xipos[self.object_bid][2] + self.lift_bonus_thresh + # Adjust init as per the specified key + init_qpos = self.sim.data.qpos.ravel().copy() + robot_init, object_init = self.ref.get_init() + if robot_init is not None: + init_qpos[:self.ref.robot_dim] = robot_init + if object_init is not None: + init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3] + init_qpos[-3:] = quat2euler(object_init[3:]) + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, frame_skip=10, + init_qpos=init_qpos, **kwargs) # Adjust horizon if not motion_extrapolation if motion_extrapolation == False: self.spec.max_episode_steps = self.ref.horizon # doesn't work always. WIP - # Adjust init as per the specified key - robot_init, object_init = self.ref.get_init() - if robot_init is not None: - self.init_qpos[:self.ref.robot_dim] = robot_init - if object_init is not None: - self.init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3] - self.init_qpos[-3:] = quat2euler(object_init[3:]) - # hack because in the super()._setup the initial posture is set to the average qpos and when a step is called, it ends in a `done` state self.initialized_pos = True # if self.sim.model.nkey>0: diff --git a/robohive/envs/myo/myomimic/myomimic_v0.py b/robohive/envs/myo/myomimic/myomimic_v0.py index c634de95..212e3bd0 100644 --- a/robohive/envs/myo/myomimic/myomimic_v0.py +++ b/robohive/envs/myo/myomimic/myomimic_v0.py @@ -82,23 +82,25 @@ def _setup(self, self.TermPose = Termimate_pose_fail ########################################## + # Adjust init as per the specified key + init_qpos = self.sim.data.qpos.ravel().copy() + robot_init, object_init = self.ref.get_init() + if robot_init is not None: + init_qpos[:self.ref.robot_dim] = robot_init + if object_init is not None: + init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3] + init_qpos[-3:] = quat2euler(object_init[3:]) + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, frame_skip=10, + init_qpos=init_qpos, **kwargs) # Adjust horizon if not motion_extrapolation if motion_extrapolation == False: self.spec.max_episode_steps = self.ref.horizon # doesn't work always. WIP - # Adjust init as per the specified key - robot_init, object_init = self.ref.get_init() - if robot_init is not None: - self.init_qpos[:self.ref.robot_dim] = robot_init - if object_init is not None: - self.init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3] - self.init_qpos[-3:] = quat2euler(object_init[3:]) - # hack because in the super()._setup the initial posture is set to the average qpos and when a step is called, it ends in a `done` state self.initialized_pos = True # if self.sim.model.nkey>0: diff --git a/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config b/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config index 8dae22fd..73318883 100644 --- a/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config +++ b/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config @@ -3,61 +3,61 @@ 'dkitty_root':{ 'interface': {'type': 'optitrack', 'server_name': '169.254.163.86', 'client_name': '169.254.163.96','port':5000, 'packet_size':36, 'id':'1'}, 'sensor':[ - {'range':(-5.00, 5.00), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'A:Tx'}, - {'range':(-5.00, 5.00), 'noise':0.005, 'adr':1, 'scale':1, 'offset':0, 'name':'A:Ty'}, - {'range':(-2.00, 2.00), 'noise':0.005, 'adr':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'}, - {'range':(-3.14, 3.14), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'A:Rx'}, - {'range':(-3.14, 3.14), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'A:Ry'}, - {'range':(-3.14, 3.14), 'noise':0.05, 'adr':5, 'scale':1, 'offset':0, 'name':'A:Rz'}, + {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'A:Tx'}, + {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'A:Ty'}, + {'range':(-2.00, 2.00), 'noise':0.005, 'hdr_adr':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'A:Rx'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'A:Ry'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':0, 'name':'A:Rz'}, ], 'actuator':[] }, # device1: sensors, actuators 'dkitty':{ - 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/DKitty"}, + 'interface': {'type': 'dynamixel', 'motor_type':"X", 'port':"/dev/DKitty"}, 'sensor':[ - {'range':(-3.419, 0.279), 'noise':0.05, 'adr':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'}, - {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'}, - {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'}, - {'range':(-0.279, 3.419), 'noise':0.05, 'adr':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'}, - {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'}, - {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'}, - {'range':(-0.279, 3.419), 'noise':0.05, 'adr':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'}, - {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'}, - {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'}, - {'range':(-3.419, 0.279), 'noise':0.05, 'adr':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'}, - {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'}, - {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':42, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ42_pos_sensor'} + {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_adr':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_adr':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_adr':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'}, + {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_adr':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':42, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ42_pos_sensor'} ], 'actuator':[ - {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10'}, - {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11'}, - {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12'}, - {'pos_range':(-0.279, 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20'}, - {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21'}, - {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22'}, - {'pos_range':(-0.279, 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30'}, - {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31'}, - {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32'}, - {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40'}, - {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41'}, - {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'} + {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10', 'mode':'Position'}, + {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11', 'mode':'Position'}, + {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12', 'mode':'Position'}, + {'pos_range':(-0.279, 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20', 'mode':'Position'}, + {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21', 'mode':'Position'}, + {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22', 'mode':'Position'}, + {'pos_range':(-0.279, 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30', 'mode':'Position'}, + {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31', 'mode':'Position'}, + {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32', 'mode':'Position'}, + {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40', 'mode':'Position'}, + {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41', 'mode':'Position'}, + {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42', 'mode':'Position'} ] # 'actuator':[ # restricted shoulder - # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10'}, - # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11'}, - # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12'}, - # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20'}, - # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21'}, - # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22'}, - # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30'}, - # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31'}, - # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32'}, - # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40'}, - # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41'}, - # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'} + # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12', 'mode':'Position'}, + # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22', 'mode':'Position'}, + # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32', 'mode':'Position'}, + # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42', 'mode':'Position'} # ] } } \ No newline at end of file diff --git a/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config b/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config index 3dad488f..e9fed7c1 100644 --- a/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config +++ b/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config @@ -3,61 +3,61 @@ 'dkitty_root':{ 'interface': {'type': 'optitrack', 'server_name': '169.254.163.86', 'client_name': '169.254.163.96','port':5000, 'packet_size':36, 'id':'1'}, 'sensor':[ - {'range':(-5.00, 5.00), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'A:Tx'}, - {'range':(-5.00, 5.00), 'noise':0.005, 'adr':1, 'scale':1, 'offset':0, 'name':'A:Ty'}, - {'range':(-2.00, 2.00), 'noise':0.005, 'adr':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'}, - {'range':(-3.14, 3.14), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'A:Rx'}, - {'range':(-3.14, 3.14), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'A:Ry'}, - {'range':(-3.14, 3.14), 'noise':0.05, 'adr':5, 'scale':1, 'offset':0, 'name':'A:Rz'}, + {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'A:Tx'}, + {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'A:Ty'}, + {'range':(-2.00, 2.00), 'noise':0.005, 'hdr_adr':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'A:Rx'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'A:Ry'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':0, 'name':'A:Rz'}, ], 'actuator':[] }, # device1: sensors, actuators 'dkitty':{ - 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/DKitty"}, + 'interface': {'type': 'dynamixel', 'motor_type':"X", 'port':"/dev/DKitty"}, 'sensor':[ - {'range':(-3.419, 0.279), 'noise':0.05, 'adr':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'}, - {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'}, - {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'}, - {'range':(-0.279, 3.419), 'noise':0.05, 'adr':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'}, - {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'}, - {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'}, - {'range':(-0.279, 3.419), 'noise':0.05, 'adr':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'}, - {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'}, - {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'}, - {'range':(-3.419, 0.279), 'noise':0.05, 'adr':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'}, - {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'}, - {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':42, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ42_pos_sensor'} + {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_adr':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_adr':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_adr':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'}, + {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_adr':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':42, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ42_pos_sensor'} ], 'actuator':[ - {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10'}, - {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11'}, - {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12'}, - {'pos_range':(-0.279, 0.75 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20'}, - {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21'}, - {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22'}, - {'pos_range':(-0.279, 0.75 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30'}, - {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31'}, - {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32'}, - {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40'}, - {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41'}, - {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'} + {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10', 'mode':'Position'}, + {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11', 'mode':'Position'}, + {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12', 'mode':'Position'}, + {'pos_range':(-0.279, 0.75 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20', 'mode':'Position'}, + {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21', 'mode':'Position'}, + {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22', 'mode':'Position'}, + {'pos_range':(-0.279, 0.75 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30', 'mode':'Position'}, + {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31', 'mode':'Position'}, + {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32', 'mode':'Position'}, + {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40', 'mode':'Position'}, + {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41', 'mode':'Position'}, + {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42', 'mode':'Position'} ] # 'actuator':[ # restricted shoulder - # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10'}, - # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11'}, - # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12'}, - # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20'}, - # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21'}, - # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22'}, - # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30'}, - # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31'}, - # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32'}, - # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40'}, - # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41'}, - # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'} + # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12', 'mode':'Position'}, + # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22', 'mode':'Position'}, + # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32', 'mode':'Position'}, + # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42', 'mode':'Position'} # ] } } \ No newline at end of file diff --git a/robohive/envs/quadrupeds/orient_v0.py b/robohive/envs/quadrupeds/orient_v0.py index 776e9978..e115cbf3 100644 --- a/robohive/envs/quadrupeds/orient_v0.py +++ b/robohive/envs/quadrupeds/orient_v0.py @@ -91,6 +91,8 @@ def _setup(self, **kwargs) # configure + # self.robot (and its robot_config) is only created inside super()._setup(), so this + # can't be computed earlier and passed via the init_qpos kwarg like other envs. for name, device in self.robot.robot_config.items(): for act_id, actuator in enumerate(device['actuator']): self.init_qpos[actuator['data_id']] = np.mean(actuator['pos_range']) diff --git a/robohive/envs/quadrupeds/walk_v0.py b/robohive/envs/quadrupeds/walk_v0.py index bf91307f..0d5d3c2d 100644 --- a/robohive/envs/quadrupeds/walk_v0.py +++ b/robohive/envs/quadrupeds/walk_v0.py @@ -92,6 +92,8 @@ def _setup(self, **kwargs) # configure + # self.robot (and its robot_config) is only created inside super()._setup(), so this + # can't be computed earlier and passed via the init_qpos kwarg like other envs. for name, device in self.robot.robot_config.items(): for act_id, actuator in enumerate(device['actuator']): self.init_qpos[actuator['data_id']] = np.mean(actuator['pos_range']) diff --git a/robohive/envs/tcdm/track.py b/robohive/envs/tcdm/track.py index 408b8c38..9101e044 100644 --- a/robohive/envs/tcdm/track.py +++ b/robohive/envs/tcdm/track.py @@ -128,23 +128,25 @@ def _setup(self, self._lift_z = self.sim.data.xipos[self.object_bid][2] + self.lift_bonus_thresh + # Adjust init as per the specified key + init_qpos = self.sim.data.qpos.ravel().copy() + robot_init, object_init = self.ref.get_init() + if robot_init is None: + init_qpos[:self.ref.robot_dim] = robot_init + if object_init is None: + init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3] + init_qpos[-3:] = quat2euler(object_init[3:]) + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, frame_skip=10, + init_qpos=init_qpos, **kwargs) # Adjust horizon if not motion_extrapolation if motion_extrapolation == False: self.spec.max_episode_steps = self.ref.horizon # doesn't work always. WIP - # Adjust init as per the specified key - robot_init, object_init = self.ref.get_init() - if robot_init is None: - self.init_qpos[:self.ref.robot_dim] = robot_init - if object_init is None: - self.init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3] - self.init_qpos[-3:] = quat2euler(object_init[3:]) - # hack because in the super()._setup the initial posture is set to the average qpos and when a step is called, it ends in a `done` state self.initialized_pos = True # if self.sim.model.nkey>0: diff --git a/robohive/robot/hardware_base.py b/robohive/robot/hardware_base.py index 6b003d69..600b9a7b 100644 --- a/robohive/robot/hardware_base.py +++ b/robohive/robot/hardware_base.py @@ -9,8 +9,56 @@ import abc import warnings +# Registry mapping robot_config's interface['type'] string -> hardwareBase subclass. +# Populated by @register_hardware(type_name) decorators on concrete hardware classes. +HARDWARE_REGISTRY = {} +SENSOR_POSTPROCESS = {} + + +def register_hardware(type_name, sensor_postprocess=None): + """Registers a hardwareBase subclass (or a factory function returning one) against an + interface['type'] string. Usable as a class decorator, or called directly with a + factory function (e.g. one that tries a preferred implementation and falls back to + an alternate one). + + sensor_postprocess(raw: dict) -> np.ndarray, optional: transforms the dict returned by + get_sensors() into other formats. + """ + def _decorator(cls_or_factory): + if isinstance(cls_or_factory, type): + assert issubclass(cls_or_factory, hardwareBase), \ + f"{cls_or_factory} must subclass hardwareBase to be registered" + HARDWARE_REGISTRY[type_name] = cls_or_factory + if sensor_postprocess is not None: + SENSOR_POSTPROCESS[type_name] = sensor_postprocess + return cls_or_factory + return _decorator + class hardwareBase(abc.ABC): + + # add tests to all defined subclasses to ensure that get_sensors() returns a dict with a 'time' key + def __init_subclass__(cls, **kwargs): + super().__init_subclass__(**kwargs) + if 'connect' in cls.__dict__: + user_connect = cls.__dict__['connect'] + + def connect_then_check(self, *args, **kw): + result = user_connect(self, *args, **kw) + try: + data = self.get_sensors() + if not (isinstance(data, dict) and 'time' in data): + warnings.warn( + f"{self.name}: get_sensors() should return a dict containing a 'time' key, got {type(data)}" + ) + except Exception as e: + warnings.warn( + f"{self.name}: could not verify get_sensors() 'time'-key contract after connect: {e}" + ) + return result + + cls.connect = connect_then_check + def __init__(self, name, *args, **kwargs) -> None: self.name = name @@ -20,7 +68,11 @@ def connect(self) -> bool: @abc.abstractmethod def okay(self) -> bool: - """Return hardware health""" + """Check if hardware is healthy and return the status""" + + @abc.abstractmethod + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc """ @abc.abstractmethod def close(self) -> bool: @@ -28,20 +80,11 @@ def close(self) -> bool: @abc.abstractmethod def reset(self) -> None: - """Reset hardware""" + """Reset hardware to a known state. Used for resetting the hardware to a known state""" @abc.abstractmethod - def _get_sensors(self) -> dict: - """Get hardware sensors — returned dict must include a 'time' key""" - def get_sensors(self) -> dict: - """Get hardware sensors, enforcing 'time' key contract""" - data = self._get_sensors() - if not (isinstance(data, dict) and 'time' in data): - warnings.warn( - f"{self.name}: get_sensors() should return a dict containing a 'time' key, got {type(data)}. " - "Please add 'time' details to your sensor data to suppress this warning.") - return data + """Get hardware sensors — should return a dict containing a 'time' key""" @abc.abstractmethod def apply_commands(self) -> None: diff --git a/robohive/robot/hardware_dynamixel.py b/robohive/robot/hardware_dynamixel.py index 16c99c55..7915cb23 100644 --- a/robohive/robot/hardware_dynamixel.py +++ b/robohive/robot/hardware_dynamixel.py @@ -5,44 +5,101 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================= """ +import time + import numpy as np from dynamixel_py import dxl -from .hardware_base import hardwareBase +from .hardware_base import hardwareBase, register_hardware +@register_hardware('dynamixel') class Dynamixels(hardwareBase): - def __init__(self, name, motor_ids, motor_type, devicename, **kwargs): + """ + Adapter around dynamixel_py's dxl client. A single dynamixel bus is shared across all + of a device's sensors and actuators, and needs per-motor mode ('Position'/'PWM') at + connect time — that per-actuator detail lives in the robot_config device dict (not in + `interface`), so this class opts in to receiving it (see Robot.hardware_init()) rather + than requiring config authors to duplicate motor IDs/modes into `interface` too. This + is a deliberately scoped exception for dynamixel-bus hardware (which genuinely needs + per-motor mode bookkeeping); other hardware classes stay interface-only. + + interface config keys: + motor_type : dynamixel motor series (e.g. 'X') + port : serial port (e.g. '/dev/ttyUSB0') + """ + + def __init__(self, name, motor_type, port, device, **kwargs): self.name = name - # initialize dynamixels - self.dxls = dxl(motor_id=motor_ids, motor_type=motor_type, devicename=devicename) - self.motor_ids = motor_ids + self.motor_type = motor_type + self.port = port + self.device = device + self.motor_ids = np.unique(device['sensor_ids'] + device['actuator_ids']).tolist() + self.dxls = None - def connect(self): + def connect(self) -> bool: """Establish hardware connection""" + self.dxls = dxl(motor_id=self.motor_ids, motor_type=self.motor_type, devicename=self.port) self.dxls.open_port() # set actuator mode - for actuator in device['actuator']: - self.dxls.set_operation_mode(motor_id=[actuator['adr']], mode=actuator['mode']) + for actuator in self.device['actuator']: + self.dxls.set_operation_mode(motor_id=[actuator['hdr_adr']], mode=actuator['mode']) # engage motors - self.dxls.engage_motor(motor_id=self.dxls, enable=True) + self.dxls.engage_motor(motor_id=self.device['actuator_ids'], enable=True) + return True - def okay(self): + def okay(self) -> bool: """Return hardware health""" + return self.dxls is not None + + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc""" + self.close() + self.connect() - def close(self): + def close(self) -> bool: """Close hardware connection""" + if self.dxls: + status = self.dxls.close(self.motor_ids) + if status: + self.dxls = None + return status + return True - def reset(self): - """Reset hardware""" + def reset(self, hw_q=None) -> None: + """Reset hardware to a known state""" + if hw_q is not None: + self.apply_commands(hw_q) - def _get_sensors(self): - """Get hardware sensors""" + def get_sensors(self) -> dict: + """Get hardware sensors. 'pos'/'vel' are positionally ordered to match + device['sensor_ids'] (i.e. device['sensor'] declaration order).""" + return { + 'time': time.time(), + 'pos': self.dxls.get_pos(self.device['sensor_ids']), + 'vel': self.dxls.get_vel(self.device['sensor_ids']), + } - def apply_commands(self): - """Apply hardware commands""" + def apply_commands(self, hw_q) -> None: + """Apply hardware commands. hw_q is positionally ordered to match device['actuator'].""" + pos_ids, pos_ctrl, pwm_ids, pwm_ctrl = [], [], [], [] + for i, actuator in enumerate(self.device['actuator']): + val = hw_q[i] + mode = actuator['mode'] + if mode == 'Position': + pos_ids.append(actuator['hdr_adr']) + pos_ctrl.append(val) + elif mode == 'PWM': + pwm_ids.append(actuator['hdr_adr']) + pwm_ctrl.append(val) + else: + raise NotImplementedError(f"Actuator mode {mode} not found") + if pos_ids: + self.dxls.set_des_pos(pos_ids, pos_ctrl) + if pwm_ids: + self.dxls.set_des_pwm(pwm_ids, pwm_ctrl) def __del__(self): - self.close() \ No newline at end of file + self.close() diff --git a/robohive/robot/hardware_franka.py b/robohive/robot/hardware_franka.py index 7a2b8f07..2a018a81 100644 --- a/robohive/robot/hardware_franka.py +++ b/robohive/robot/hardware_franka.py @@ -15,7 +15,7 @@ from polymetis import RobotInterface import torchcontrol as toco -from robohive.robot.hardware_base import hardwareBase +from robohive.robot.hardware_base import hardwareBase, register_hardware from robohive.utils.min_jerk import generate_joint_space_min_jerk import argparse @@ -56,6 +56,7 @@ def forward(self, state_dict: Dict[str, torch.Tensor]): +@register_hardware('franka') class FrankaArm(hardwareBase): def __init__(self, name, ip_address, gain_scale=1.0, reset_gain_scale=1.0, **kwargs): self.name = name @@ -90,7 +91,7 @@ def connect(self, policy=None): # Create policy instance s_initial = self.get_sensors() policy = JointPDPolicy( - desired_joint_pos=s_initial['joint_pos'], + desired_joint_pos=s_initial['pos'], kp=self.gain_scale * torch.Tensor(self.robot.metadata.default_Kq), kd=self.gain_scale * torch.Tensor(self.robot.metadata.default_Kqd), ) @@ -144,6 +145,12 @@ def reconnect(self): print("Re-connection success") + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc""" + self.reconnect() + self.reset() + + def reset(self, reset_pos=None, time_to_go=5): """Reset hardware""" @@ -157,7 +164,7 @@ def reset(self, reset_pos=None, time_to_go=5): reset_pos = torch.Tensor(reset_pos) # Use registered controller - q_current = self.get_sensors()['joint_pos'] + q_current = self.get_sensors()['pos'] # generate min jerk trajectory dt = 0.1 waypoints = generate_joint_space_min_jerk(start=q_current, goal=reset_pos, time_to_go=time_to_go, dt=dt) @@ -185,7 +192,7 @@ def reset(self, reset_pos=None, time_to_go=5): self.reset(reset_pos, time_to_go) - def _get_sensors(self) -> dict: + def get_sensors(self) -> dict: """Get hardware sensors""" try: joint_pos = self.robot.get_joint_positions() @@ -193,8 +200,8 @@ def _get_sensors(self) -> dict: except: print("Failed to get current sensors: ", end="") self.reconnect() - return self._get_sensors() - return {'time': time.time(), 'joint_pos': joint_pos, 'joint_vel': joint_vel} + return self.get_sensors() + return {'time': time.time(), 'pos': joint_pos, 'vel': joint_vel} def apply_commands(self, q_desired=None, kp=None, kd=None): @@ -252,8 +259,8 @@ def get_args(): # Update policy to execute a sine trajectory on joint 6 for 5 seconds print("Starting sine motion updates...") s_initial = franka.get_sensors() - q_initial = s_initial['joint_pos'].clone() - q_desired = s_initial['joint_pos'].clone() + q_initial = s_initial['pos'].clone() + q_desired = s_initial['pos'].clone() for i in range(int(time_to_go * hz)): q_desired[5] = q_initial[5] + m * np.sin(np.pi * i / (T * hz)) diff --git a/robohive/robot/hardware_optitrack.py b/robohive/robot/hardware_optitrack.py index 5aab551b..b7cf58fa 100644 --- a/robohive/robot/hardware_optitrack.py +++ b/robohive/robot/hardware_optitrack.py @@ -5,7 +5,8 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================= """ -from darwin.darwin_robot.hardware_base import hardwareBase +from robohive.robot.hardware_base import hardwareBase, register_hardware +from robohive.utils.quat_math import quat2euler import numpy as np import socket import argparse @@ -15,6 +16,17 @@ _USE_UDP = True + +def _optitrack_sensor_postprocess(raw): + c, b, a = quat2euler(raw['quat']) + rx = np.pi - a + rx = (rx - 2*np.pi) if rx > np.pi else rx + ry = b + rz = -c + return np.concatenate([raw['pos'], np.array([rx, ry, rz])]) + + +@register_hardware('optitrack', sensor_postprocess=_optitrack_sensor_postprocess) class OptiTrack(hardwareBase): """ OptiTrack Client: Connects to the server and receives streaming data @@ -27,9 +39,11 @@ class OptiTrack(hardwareBase): # Cached client that is shared for the application lifetime. _OPTI_CLIENT = None - def __init__(self, ip: str, port:int=5000, packet_size:int=36, cache_maxsize:int=0): + def __init__(self, name='optitrack', ip: str = None, client_name: str = None, port:int=5000, packet_size:int=36, cache_maxsize:int=0, **kwargs): + self.name = name if self._OPTI_CLIENT is None: - self.ip = ip + # robot_config's interface dict historically names this key 'client_name' + self.ip = ip if ip is not None else client_name self.port = port self.packet_size = packet_size self._sensor_cache_maxsize = cache_maxsize @@ -93,6 +107,12 @@ def reset(self): self.connect() + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc""" + self.close() + self.connect() + + # [t, id, x, y, z, q0, q1, q2, q3, q4] def read_sensor(self): # receive response @@ -134,7 +154,7 @@ def __repr__(self): self.data_float[2], self.data_float[3], self.data_float[4]) # get latest sensor value (helpful when there is a single sensors) - def _get_sensors(self) -> dict: + def get_sensors(self) -> dict: # sensor_data isn't updated in place ==> it can be easily passed around and cached # repeated calls will return the same data_frame ==> no overhead for multiple queries to the same sensor reading return self.sensor_data diff --git a/robohive/robot/hardware_realsense.py b/robohive/robot/hardware_realsense.py index faa68e5f..7616e735 100644 --- a/robohive/robot/hardware_realsense.py +++ b/robohive/robot/hardware_realsense.py @@ -1,6 +1,6 @@ import numpy as np # from hardware_base import hardwareBase -from robohive.robot.hardware_base import hardwareBase +from robohive.robot.hardware_base import hardwareBase, register_hardware import argparse import a0 @@ -19,6 +19,7 @@ class RealSense(hardwareBase): def __init__(self, name, rgb_topic=None, d_topic=None, **kwargs): assert rgb_topic or d_topic, "Atleast one of the topics is needed" + self.name = name self.rgb_topic = rgb_topic self.d_topic = d_topic self.last_image_pkt = None @@ -27,6 +28,10 @@ def __init__(self, name, rgb_topic=None, d_topic=None, **kwargs): self.most_recent_pkt_ts = None self.timeout = 1 # in seconds + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc""" + self.connect() + def connect(self): # sub to the topics if self.rgb_topic: @@ -58,7 +63,7 @@ def callback(self, pkt): timestamp_str_wo_nano = timestamp_str[:23] + timestamp_str[29:] self.most_recent_pkt_ts = datetime.datetime.fromisoformat(timestamp_str_wo_nano) - def _get_sensors(self) -> dict: + def get_sensors(self) -> dict: # get all data from all topics last_img = copy.deepcopy(self.last_image_pkt) last_depth = copy.deepcopy(self.last_depth_pkt) @@ -96,6 +101,17 @@ def reset(self): return 0 +def _realsense_factory(name, **iface): + """Try the a0-based RealSense first, fall back to the direct pyrealsense2 wrapper.""" + try: + return RealSense(name=name, **iface) + except Exception: + from .hardware_realsense_single import RealsenseAPI + return RealsenseAPI(name=name, **iface) + + +register_hardware('realsense')(_realsense_factory) + # Get inputs from user def get_args(): diff --git a/robohive/robot/hardware_realsense_single.py b/robohive/robot/hardware_realsense_single.py index cb18472d..e64ec1a3 100644 --- a/robohive/robot/hardware_realsense_single.py +++ b/robohive/robot/hardware_realsense_single.py @@ -2,11 +2,14 @@ import pyrealsense2 as rs from collections import OrderedDict +from robohive.robot.hardware_base import hardwareBase -class RealsenseAPI: + +class RealsenseAPI(hardwareBase): """Wrapper that implements boilerplate code for RealSense cameras""" - def __init__(self, device_id=None, height=480, width=640, fps=30, warm_start=30, type=None,): + def __init__(self, name='realsense', device_id=None, height=480, width=640, fps=30, warm_start=30, type=None, **kwargs): + self.name = name self.height = height self.width = width self.fps = fps @@ -73,6 +76,10 @@ def get_sensors(self): def okay(self): return True + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc""" + self.connect() + def apply_commands(self): return 0 diff --git a/robohive/robot/hardware_robotiq.py b/robohive/robot/hardware_robotiq.py index 76182c8d..df10a837 100644 --- a/robohive/robot/hardware_robotiq.py +++ b/robohive/robot/hardware_robotiq.py @@ -1,11 +1,12 @@ from enum import Flag from polymetis import GripperInterface -from robohive.robot.hardware_base import hardwareBase +from robohive.robot.hardware_base import hardwareBase, register_hardware import numpy as np import argparse import time +@register_hardware('robotiq') class Robotiq(hardwareBase): def __init__(self, name, ip_address, **kwargs): self.name = name @@ -81,6 +82,11 @@ def reconnect(self): print("RBQ:> Re-connection success") + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc""" + self.reconnect() + + def reset(self, width=None, **kwargs): """Reset hardware""" if not width: @@ -88,17 +94,20 @@ def reset(self, width=None, **kwargs): self.apply_commands(width=width, **kwargs) - def _get_sensors(self) -> dict: + def get_sensors(self) -> dict: """Get hardware sensors""" try: curr_state = self.robot.get_state() except: print("RBQ:> Failed to get current sensors: ", end="") self.reconnect() - return self._get_sensors() - return {'time': time.time(), 'width': np.array([curr_state.width])} + return self.get_sensors() + return {'time': time.time(), 'pos': np.array([curr_state.width])} - def apply_commands(self, width:float, speed:float=0.1, force:float=0.1): + def apply_commands(self, width, speed:float=0.1, force:float=0.1): + # width may be a scalar or a length-1 array-like (Robot.hardware_apply_controls + # always passes an array positionally matching this device's single actuator). + width = float(np.asarray(width).reshape(-1)[0]) assert width>=0.0 and width<=self.max_width, "Gripper desired width ({}) is out of bound (0,{})".format(width, self.max_width) self.robot.goto(width=width, speed=speed, force=force) return 0 diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index 12e5338e..741e7feb 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -5,6 +5,7 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================= """ +import inspect import os import time from collections import deque @@ -13,8 +14,8 @@ import numpy as np from robohive.physics.sim_scene import SimScene +from robohive.robot.hardware_base import HARDWARE_REGISTRY, SENSOR_POSTPROCESS from robohive.utils.prompt_utils import Prompt, prompt -from robohive.utils.quat_math import quat2euler np.set_printoptions(precision=4) @@ -35,9 +36,9 @@ # nq should be nv # Order of sensors and actuators in config should follow XML order # Space definitions - # sim_id: ID of the sensor/actuator in the sim - # hdr_id: ID of the sensor/actuator in the robot_config (hardware) space (robot_config unifies different hardware into a single unified hardware space) - # adr: Address of the sensor/actuator in the individual hardware space (e.g. dynamixel) (This is the address used during communicate with the individual hardware) + # sim_id: ID (defined by order in mujoco's xml) of the sensor/actuator in the sim + # hdr_id: ID (defined by the order in .config) of the sensor/actuator in the robot_config (hardware) space (robot_config unifies different hardware into a single unified hardware space) + # hdr_adr: Physical address of the sensor/actuator (defined by hardware specs) (e.g. dynamixel - This is the address/motor_id used to communicate with the individual motors) @@ -127,63 +128,26 @@ def hardware_init(self, robot_config): # initalize for name, device in robot_config.items(): prompt("Initializing device: %s"%(name), 'white', 'on_grey') - if device['interface']['type'] == 'dynamixel': - # initialize dynamixels - from dynamixel_py import dxl - ids = np.unique([device['sensor_ids'] + device['actuator_ids']]).tolist() - device['robot'] = dxl(motor_id=ids, motor_type=\ - device['interface']['motor_type'], devicename= device['interface']['name']) - - # from .hardware_dynamixel import Dynamixels - # motor_ids = np.unique([device['sensor_ids'] + device['actuator_ids']]).tolist() - # device['robot'] = Dynamixels(name=name, motor_ids=motor_ids, motor_type=device['interface']['motor_type'], devicename= device['interface']['name']) - - elif device['interface']['type'] == 'optitrack': - from .hardware_optitrack import OptiTrack - device['robot'] = OptiTrack(ip=device['interface']['client_name'], \ - port=device['interface']['port'], packet_size=device['interface']['packet_size']) - - elif device['interface']['type'] == 'franka': - from .hardware_franka import FrankaArm - device['robot'] = FrankaArm(name=name, **device['interface']) - - elif device['interface']['type'] == 'realsense': - try: - from .hardware_realsense import RealSense - device['robot'] = RealSense(name=name, **device['interface']) - except: - from .hardware_realsense_single import RealsenseAPI - device['robot'] = RealsenseAPI(**device['interface']) - - elif device['interface']['type'] == 'robotiq': - from .hardware_robotiq import Robotiq - device['robot'] = Robotiq(name=name, **device['interface']) - - else: - print("ERROR: interface ({}) not found".format(device['interface']['type'])) - raise NotImplemented + itype = device['interface']['type'] + cls = HARDWARE_REGISTRY.get(itype) + if cls is None: + raise NotImplementedError( + "No hardwareBase registered for interface.type={!r}. Registered types: {}. " + "Import the module that registers this type before constructing " + "Robot(is_hardware=True).".format(itype, sorted(HARDWARE_REGISTRY))) + iface = {k: v for k, v in device['interface'].items() if k != 'type'} + # Most hardware classes are self-contained (ip/port/etc. from `interface` is + # enough). A few (dynamixel-bus devices, which manage several motors sharing + # one connection and need per-motor address/mode bookkeeping) opt in to + # receiving the full robot_config device dict by naming `device` as a + # constructor parameter; everyone else never sees it. + if 'device' in inspect.signature(cls).parameters: + iface['device'] = device + device['robot'] = cls(name=name, **iface) # start all hardware for name, device in robot_config.items(): - - # Dynamixels - if device['interface']['type'] == 'dynamixel': - device['robot'].open_port() - - # set actuator mode - for actuator in device['actuator']: - device['robot'].set_operation_mode(motor_id=[actuator['adr']], mode=actuator['mode']) - - # engage motors - device['robot'].engage_motor(motor_id=device['actuator_ids'], enable=True) - - # Other devices - elif device['interface']['type'] in ['optitrack', 'franka', 'realsense', 'robotiq']: - device['robot'].connect() - - else: - print("ERROR: interface ({}) not found".format(device['interface']['type'])) - raise NotImplementedError + device['robot'].connect() return robot_config @@ -194,115 +158,67 @@ def hardware_get_sensors(self): current_sensor_value['time'] = time.time() - self.time_start for name, device in self.robot_config.items(): if 'sensor' in device.keys() and len(device['sensor'])>0: - # get sensors - if device['interface']['type'] == 'dynamixel': - # TODO: choose between pos, vel, or posvel - current_sensor_value[name] = device['robot'].get_pos(device['sensor_ids']) - current_sensor_value[name+'_vel'] = device['robot'].get_vel(device['sensor_ids']) - - elif device['interface']['type'] == 'optitrack': - data = device['robot'].get_sensors() - c, b, a = quat2euler(data['quat']) - rx = np.pi - a - rx = (rx - 2*np.pi) if rx > np.pi else rx - ry = b - rz = -c - # print("Pos:", x, y, z) - # print("Rotations:", rx, ry, rz) - current_sensor_value[name] = np.concatenate([data['pos'], np.array([rx, ry, rz])]) - # current_sensor_value[name] = np.array([x, y, z, 0, 0, 0]) - # current_sensor_value[name] = np.array([x, y, z, -(a+np.pi/2), -c, -b]) - - elif device['interface']['type'] == 'franka': - sensors = device['robot'].get_sensors() - current_sensor_value[name] = np.concatenate([sensors['joint_pos'], sensors['joint_vel']]) - - elif device['interface']['type'] == 'robotiq': - sensors = device['robot'].get_sensors() - current_sensor_value[name] = sensors - - else: - print("ERROR: interface ({}) not found".format(device['interface']['type'])) - raise NotImplementedError - - # calibrate sensors + itype = device['interface']['type'] + raw = device['robot'].get_sensors() + postprocess = SENSOR_POSTPROCESS.get(itype, lambda raw: raw['pos']) + vals = postprocess(raw) + + # calibrate sensors. vals is positionally ordered to match device['sensor'] + # (both built by iterating the same config-declared list, see configure_robot()). + arr = np.empty(len(device['sensor']), dtype=np.float64) for id, sensor in enumerate(device['sensor']): - current_sensor_value[name][id] = current_sensor_value[name][id]*sensor['scale'] + sensor['offset'] - device['sensor_data'] = current_sensor_value[name] + arr[id] = vals[id]*sensor['scale'] + sensor['offset'] + current_sensor_value[name] = arr + device['sensor_data'] = arr device['sensor_time'] = current_sensor_value['time'] return current_sensor_value # apply controls to hardware - def hardware_apply_controls(self, control, space='hdr', is_reset=False): + def hardware_apply_controls(self, control, space='hdr'): """ + Send one dt's worth of (already-clipped, locally-achievable) controls to hardware. control: control vector in hdr or sim space space: 'hdr' or 'sim' (defaults to 'hdr' as represented in robot_config) - is_reset: if True, reset the hardware to the control values """ for name, device in self.robot_config.items(): if 'actuator' in device.keys() and len(device['actuator'])>0: - if device['interface']['type'] == 'dynamixel': - # group as per mode - pos_ctrl = [] - pos_ids = [] - pwm_ctrl = [] - pwm_ids = [] - for actuator in device['actuator']: - ctrl = control[actuator['sim_id']] if space == 'sim' else control[actuator['hdr_id']] - # calibrate - calib_ctrl = ctrl*actuator['scale']+ actuator['offset'] - if actuator['mode'] == 'Position': - pos_ids.append(actuator['adr']) - pos_ctrl.append(calib_ctrl) - elif actuator['mode'] == 'PWM': - pwm_ids.append(actuator['adr']) - pwm_ctrl.append(calib_ctrl) - else: - print("ERROR: Mode not found") - raise NotImplementedError(f"ERROR: Actuator mode {actuator['mode']} not found") - # send controls - if pos_ids: - device['robot'].set_des_pos(pos_ids, pos_ctrl) - if pwm_ids: - device['robot'].set_des_pwm(pwm_ids, pwm_ctrl) - - elif device['interface']['type'] in ['franka', 'robotiq']: - des_pos = [] - for actuator in device['actuator']: - ctrl = control[actuator['sim_id']] if space == 'sim' else control[actuator['hdr_id']] - # calibrate - des_pos.append(ctrl*actuator['scale']+ actuator['offset']) - if is_reset: - device['robot'].reset(des_pos) - else: - device['robot'].apply_commands(des_pos) - else: - raise NotImplementedError("ERROR: interface not found") + # hw_q is positionally ordered to match device['actuator'] (both built by + # iterating the same config-declared list, see configure_robot()). + hw_q = [] + for actuator in device['actuator']: + ctrl = control[actuator['sim_id']] if space == 'sim' else control[actuator['hdr_id']] + hw_q.append(ctrl*actuator['scale'] + actuator['offset']) + device['robot'].apply_commands(hw_q) + # move actuated dofs to a target position (blocking, large-displacement — distinct from + # the per-dt hardware_apply_controls above; hardware classes implement this via their own + # min-jerk/via-point trajectories) + def hardware_reset(self, reset_pos): + for name, device in self.robot_config.items(): + if name == 'default_robot': + continue + qpos_actuators = [a for a in device.get('actuator', []) if a['data_type'] == 'qpos'] + if qpos_actuators: + hw_q = [np.clip(reset_pos[a['data_id']], a['pos_range'][0], a['pos_range'][1]) + for a in qpos_actuators] + device['robot'].reset(hw_q) + else: + # passive device (tendon-driven gripper, camera, etc.) — no qpos target to + # compute; let the device bring itself to its own known reset state + device['robot'].reset() + # close hardware def hardware_close(self): status = True for name, device in self.robot_config.items(): - if device['interface']['type'] == 'dynamixel': - if device['robot']: - print("Closing dynamixel connection") - ids = np.unique([device['sensor_ids'] + device['actuator_ids']]).tolist() - status = device['robot'].close(ids) - if status is True: - device['robot']= None - elif device['interface']['type'] in ['optitrack', 'franka', 'realsense', 'robotiq']: - if device['robot']: - print("Closing {} connection".format(device['interface']['type'])) - status = device['robot'].close() - if status is True: - device['robot']= None - else: - print("ERROR: interface not found") - raise NotImplemented - + if device.get('robot'): + print("Closing {} connection".format(device['interface']['type'])) + status = device['robot'].close() + if status is True: + device['robot'] = None return status @@ -333,7 +249,7 @@ def configure_robot(self, sim, config_path): sensor['hdr_id'] = hdr_sensor_id sensor['sim_id'] = sim.model.sensor_name2id(sensor['name']) device['sensor_names'].append(sensor['name']) # list of all ids - device['sensor_ids'].append(sensor['adr']) # list of all ids + device['sensor_ids'].append(sensor['hdr_adr']) # list of all ids sensor_type = sim.model.sensor_type[sensor['sim_id']] sensor_objid = sim.model.sensor_objid[sensor['sim_id']] # sensordata_id: address in sim.data.sensordata for this sensor. @@ -360,7 +276,7 @@ def configure_robot(self, sim, config_path): actuator['hdr_id'] = hdr_actuator_id actuator['sim_id'] = sim.model.actuator_name2id(actuator['name']) device['actuator_names'].append(actuator['name']) # list of all ids - device['actuator_ids'].append(actuator['adr']) # list of all ids + device['actuator_ids'].append(actuator['hdr_adr']) # list of all ids actuator_trntype = sim.model.actuator_trntype[actuator['sim_id']] actuator_trnid = sim.model.actuator_trnid[actuator['sim_id'], 0] if actuator_trntype == mujoco.mjtTrn.mjTRN_JOINT: # // force on joint @@ -456,8 +372,14 @@ def get_visual_sensors(self, height:int, width:int, cameras:list, device_id:int, for ind, cam_name in enumerate(cameras): assert cam_name in self.robot_config.keys(), "{} camera not found".format(cam_name) device = self.robot_config[cam_name] - assert device['interface']['type'] == 'realsense', "Check interface type for {}".format(cam) - data = device['robot'].get_sensors() + + if hasattr(device['robot'], 'get_frame'): + # RGB-only camera (e.g. a UVC/webcam) — no depth stream. + rgb = device['robot'].get_frame() + data = {'time': time.time() - self.time_start, 'rgb': rgb, 'd': None} + else: + # RealSense-style camera: get_sensors() itself returns {'rgb','d'}. + data = device['robot'].get_sensors() data_height = data['rgb'].shape[0] assert data_height == height, "Incorrect image height: required:{}, found:{}".format(height, data_height) data_width = data['rgb'].shape[1] @@ -466,11 +388,12 @@ def get_visual_sensors(self, height:int, width:int, cameras:list, device_id:int, # calibrate sensors for cam in device['cam']: - current_sensor_value[cam_name][cam['adr']] = current_sensor_value[cam_name][cam['adr']]*cam['scale'] + cam['offset'] + current_sensor_value[cam_name][cam['hdr_adr']] = current_sensor_value[cam_name][cam['hdr_adr']]*cam['scale'] + cam['offset'] device['sensor_data'] = current_sensor_value[cam_name] device['sensor_time'] = current_sensor_value['time'] imgs[ind, :, :, :] = current_sensor_value[cam_name]['rgb'] - depths[ind, :, :] = current_sensor_value[cam_name]['d'][:,:,0] # assumes single channel depth + if current_sensor_value[cam_name]['d'] is not None: + depths[ind, :, :] = current_sensor_value[cam_name]['d'][:,:,0] # assumes single channel depth else: imgs = np.zeros((len(cameras), height, width, 3), dtype=np.uint8) @@ -759,14 +682,12 @@ def reset(self, # for passive dofs => sensor specs feasibe_pos = reset_pos.copy() feasibe_vel = reset_vel.copy() - ctrl_feasible=[] for name, device in self.robot_config.items(): if name != "default_robot": if len(device['actuator'])>0: # actuated dofs for actuator in device['actuator']: if actuator['data_type'] == 'qpos': feasibe_pos[actuator['data_id']] = np.clip(reset_pos[actuator['data_id']], actuator['pos_range'][0], actuator['pos_range'][1]) - ctrl_feasible.append(feasibe_pos[actuator['data_id']]) else: # passive dofs for sensor in device['sensor']: if sensor['data_type'] == 'qpos': @@ -778,11 +699,8 @@ def reset(self, t_reset_start = time.time() prompt("\nRollout took:{}".format(t_reset_start- self.time_start)) prompt("\aResetting {}: ".format(self.name), 'white', 'on_grey', flush=True, end="") - # send request to the actuated dofs - self.hardware_apply_controls(ctrl_feasible, is_reset=True) - - # engage other reset mechanisms for passive dofs - # TODO raise NotImplementedError + # send request to all devices, actuated and passive alike + self.hardware_reset(feasibe_pos) if blocking: input("press a key to start rollout") diff --git a/setup.py b/setup.py index bbfac246..88c5a0aa 100644 --- a/setup.py +++ b/setup.py @@ -60,7 +60,7 @@ def package_files(directory): install_requires=[ "click", # 'gym==0.13', # default to this stable point if caught in gym issues. - "gymnasium==0.29.1", + "gymnasium>=0.29.1", "mujoco==3.3.3", "numpy>=2", "dm-control==1.0.31",