def setup_arm_mp(self):
"""
Set up arm motion planner
"""
if self.robot_type == 'Fetch':
self.arm_default_joint_positions = (0.10322468280792236,
-1.414019864768982,
1.5178184935241699,
0.8189625336474915,
2.200358942909668,
2.9631312579803466,
-1.2862852996643066,
0.0008453550418615341)
self.arm_joint_ids = joints_from_names(self.robot_id, [
'torso_lift_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'upperarm_roll_joint',
'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint',
'wrist_roll_joint'
])
elif self.robot_type == 'Movo':
self.arm_default_joint_positions = (0.205, -1.50058731470836,
-1.3002625076695704,
0.5204845864369407,
-2.6923805472917626,
-0.02678584326934146,
0.5065742552588746,
-1.562883631882778)
self.arm_joint_ids = joints_from_names(self.robot_id, [
"linear_joint",
"right_shoulder_pan_joint",
"right_shoulder_lift_joint",
"right_arm_half_joint",
"right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint",
"right_wrist_3_joint",
])
self.arm_joint_ids_all = get_moving_links(self.robot_id,
self.arm_joint_ids)
self.arm_joint_ids_all = [
item for item in self.arm_joint_ids_all
if item != self.robot.end_effector_part_index()
]
self.arm_ik_threshold = 0.05
self.mp_obstacles = []
if type(self.env.scene) == StaticIndoorScene:
if self.env.scene.mesh_body_id is not None:
self.mp_obstacles.append(self.env.scene.mesh_body_id)
elif type(self.env.scene) == InteractiveIndoorScene:
self.mp_obstacles.extend(self.env.scene.get_body_ids())
def robot_specific_reset(self):
super(Fetch, self).robot_specific_reset()
# roll the arm to its body
robot_id = self.robot_ids[0]
arm_joints = joints_from_names(robot_id, [
'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint',
'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint',
'wrist_flex_joint', 'wrist_roll_joint'
])
rest_position = (0.02, np.pi / 2.0 - 0.4, np.pi / 2.0 - 0.1, -0.4,
np.pi / 2.0 + 0.1, 0.0, np.pi / 2.0, 0.0)
# might be a better pose to initiate manipulation
# rest_position = (0.30322468280792236, -1.414019864768982,
# 1.5178184935241699, 0.8189625336474915,
# 2.200358942909668, 2.9631312579803466,
# -1.2862852996643066, 0.0008453550418615341)
set_joint_positions(robot_id, arm_joints, rest_position)
def load(self):
"""
Load the robot into pybullet. Filter out unnecessary self collision
due to modeling imperfection in the URDF
"""
ids = super(JR2_Kinova, self).load()
robot_id = self.robot_ids[0]
disable_collision_names = [
['base_chassis_joint', 'pan_joint'],
['base_chassis_joint', 'tilt_joint'],
['base_chassis_joint', 'camera_joint'],
['jr2_fixed_body_joint', 'pan_joint'],
['jr2_fixed_body_joint', 'tilt_joint'],
['jr2_fixed_body_joint', 'camera_joint'],
]
for names in disable_collision_names:
link_a, link_b = joints_from_names(robot_id, names)
p.setCollisionFilterPair(robot_id, robot_id, link_a, link_b, 0)
return ids
def load(self):
"""
Load the robot into pybullet. Filter out unnecessary self collision
due to modeling imperfection in the URDF
"""
ids = super(Fetch, self).load()
robot_id = self.robot_ids[0]
disable_collision_names = [
['torso_lift_joint', 'shoulder_lift_joint'],
['torso_lift_joint', 'torso_fixed_joint'],
['caster_wheel_joint', 'estop_joint'],
['caster_wheel_joint', 'laser_joint'],
['caster_wheel_joint', 'torso_fixed_joint'],
['caster_wheel_joint', 'l_wheel_joint'],
['caster_wheel_joint', 'r_wheel_joint'],
]
for names in disable_collision_names:
link_a, link_b = joints_from_names(robot_id, names)
p.setCollisionFilterPair(robot_id, robot_id, link_a, link_b, 0)
return ids
def main():
config = parse_config('../configs/fetch_p2p_nav.yaml')
s = Simulator(mode='gui', physics_timestep=1 / 240.0)
scene = EmptyScene()
s.import_scene(scene)
fetch = Fetch(config)
s.import_robot(fetch)
robot_id = fetch.robot_ids[0]
arm_joints = joints_from_names(robot_id, [
'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint',
'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint',
'wrist_flex_joint', 'wrist_roll_joint'
])
#finger_joints = joints_from_names(robot_id, ['l_gripper_finger_joint', 'r_gripper_finger_joint'])
fetch.robot_body.reset_position([0, 0, 0])
fetch.robot_body.reset_orientation([0, 0, 1, 0])
x, y, z = fetch.get_end_effector_position()
#set_joint_positions(robot_id, finger_joints, [0.04,0.04])
print(x, y, z)
visual_marker = p.createVisualShape(p.GEOM_SPHERE, radius=0.02)
marker = p.createMultiBody(baseVisualShapeIndex=visual_marker)
#max_limits = [0,0] + get_max_limits(robot_id, arm_joints) + [0.05,0.05]
#min_limits = [0,0] + get_min_limits(robot_id, arm_joints) + [0,0]
#rest_position = [0,0] + list(get_joint_positions(robot_id, arm_joints)) + [0.04,0.04]
max_limits = [0, 0] + get_max_limits(robot_id, arm_joints)
min_limits = [0, 0] + get_min_limits(robot_id, arm_joints)
rest_position = [0, 0] + list(get_joint_positions(robot_id, arm_joints))
joint_range = list(np.array(max_limits) - np.array(min_limits))
joint_range = [item + 1 for item in joint_range]
jd = [0.1 for item in joint_range]
print(max_limits)
print(min_limits)
def accurateCalculateInverseKinematics(robotid, endEffectorId, targetPos,
threshold, maxIter):
sample_fn = get_sample_fn(robotid, arm_joints)
set_joint_positions(robotid, arm_joints, sample_fn())
it = 0
while it < maxIter:
jointPoses = p.calculateInverseKinematics(robotid,
endEffectorId,
targetPos,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=joint_range,
restPoses=rest_position,
jointDamping=jd)
set_joint_positions(robotid, arm_joints, jointPoses[2:10])
ls = p.getLinkState(robotid, endEffectorId)
newPos = ls[4]
dist = np.linalg.norm(np.array(targetPos) - np.array(newPos))
if dist < threshold:
break
it += 1
print("Num iter: " + str(it) + ", threshold: " + str(dist))
return jointPoses
while True:
with Profiler("Simulation step"):
fetch.robot_body.reset_position([0, 0, 0])
fetch.robot_body.reset_orientation([0, 0, 1, 0])
threshold = 0.01
maxIter = 100
joint_pos = accurateCalculateInverseKinematics(
robot_id, fetch.parts['gripper_link'].body_part_index,
[x, y, z], threshold, maxIter)[2:10]
#set_joint_positions(robot_id, finger_joints, [0.04, 0.04])
s.step()
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_IS_DOWN)):
x += 0.01
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_IS_DOWN)):
x -= 0.01
if (k == p.B3G_UP_ARROW and (v & p.KEY_IS_DOWN)):
y += 0.01
if (k == p.B3G_DOWN_ARROW and (v & p.KEY_IS_DOWN)):
y -= 0.01
if (k == ord('z') and (v & p.KEY_IS_DOWN)):
z += 0.01
if (k == ord('x') and (v & p.KEY_IS_DOWN)):
z -= 0.01
p.resetBasePositionAndOrientation(marker, [x, y, z], [0, 0, 0, 1])
s.disconnect()