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
def execute_arm_push(self, plan, hit_pos, hit_normal):
"""
Execute arm push given arm trajectory
Should be called after plan_arm_push()
:param plan: arm trajectory or None if no plan can be found
:param hit_pos: 3D position to reach
:param hit_normal: direction to push after reacehing that position
"""
if plan is not None:
self.dry_run_arm_plan(plan)
self.interact(hit_pos, hit_normal)
set_joint_positions(self.robot_id, self.arm_joint_ids,
self.arm_default_joint_positions)
self.simulator_sync()
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 plan_arm_push(self, hit_pos, hit_normal):
"""
Attempt to reach a 3D position and prepare for a push later
:param hit_pos: 3D position to reach
:param hit_normal: direction to push after reacehing that position
:return: arm trajectory or None if no plan can be found
"""
if self.marker is not None:
self.set_marker_position_direction(hit_pos, hit_normal)
joint_positions = self.get_arm_joint_positions(hit_pos)
#print('planned JP', joint_positions)
set_joint_positions(self.robot_id, self.arm_joint_ids,
self.arm_default_joint_positions)
self.simulator_sync()
if joint_positions is not None:
plan = self.plan_arm_motion(joint_positions)
return plan
else:
return None
def dry_run_arm_plan(self, arm_path):
"""
Dry run arm motion plan by setting the arm joint position without physics simulation
:param arm_path: arm trajectory or None if no plan can be found
"""
base_pose = get_base_values(self.robot_id)
if arm_path is not None:
if self.mode in ['gui', 'iggui', 'pbgui']:
for joint_way_point in arm_path:
set_joint_positions(self.robot_id, self.arm_joint_ids,
joint_way_point)
set_base_values_with_z(self.robot_id,
base_pose,
z=self.initial_height)
self.simulator_sync()
# sleep(0.02) # animation
else:
set_joint_positions(self.robot_id, self.arm_joint_ids,
arm_path[-1])
else:
# print('arm mp fails')
if self.robot_type == 'Movo':
self.robot.tuck()
set_joint_positions(self.robot_id, self.arm_joint_ids,
self.arm_default_joint_positions)
def get_arm_joint_positions(self, arm_ik_goal):
"""
Attempt to find arm_joint_positions that satisfies arm_subgoal
If failed, return None
:param arm_ik_goal: [x, y, z] in the world frame
:return: arm joint positions
"""
ik_start = time()
max_limits, min_limits, rest_position, joint_range, joint_damping = \
self.get_ik_parameters()
n_attempt = 0
max_attempt = 75
sample_fn = get_sample_fn(self.robot_id, self.arm_joint_ids)
base_pose = get_base_values(self.robot_id)
state_id = p.saveState()
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, False)
# find collision-free IK solution for arm_subgoal
while n_attempt < max_attempt:
if self.robot_type == 'Movo':
self.robot.tuck()
set_joint_positions(self.robot_id, self.arm_joint_ids, sample_fn())
arm_joint_positions = p.calculateInverseKinematics(
self.robot_id,
self.robot.end_effector_part_index(),
targetPosition=arm_ik_goal,
# targetOrientation=self.robots[0].get_orientation(),
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=joint_range,
restPoses=rest_position,
jointDamping=joint_damping,
solver=p.IK_DLS,
maxNumIterations=100)
if self.robot_type == 'Fetch':
arm_joint_positions = arm_joint_positions[2:10]
elif self.robot_type == 'Movo':
arm_joint_positions = arm_joint_positions[:8]
set_joint_positions(self.robot_id, self.arm_joint_ids,
arm_joint_positions)
dist = l2_distance(self.robot.get_end_effector_position(),
arm_ik_goal)
# print('dist', dist)
if dist > self.arm_ik_threshold:
n_attempt += 1
continue
# need to simulator_step to get the latest collision
self.simulator_step()
# simulator_step will slightly move the robot base and the objects
set_base_values_with_z(self.robot_id,
base_pose,
z=self.initial_height)
# self.reset_object_states()
# TODO: have a princpled way for stashing and resetting object states
# arm should not have any collision
if self.robot_type == 'Movo':
collision_free = is_collision_free(
body_a=self.robot_id, link_a_list=self.arm_joint_ids_all)
# ignore linear link
else:
collision_free = is_collision_free(
body_a=self.robot_id, link_a_list=self.arm_joint_ids)
if not collision_free:
n_attempt += 1
# print('arm has collision')
continue
# gripper should not have any self-collision
collision_free = is_collision_free(
body_a=self.robot_id,
link_a_list=[self.robot.end_effector_part_index()],
body_b=self.robot_id)
if not collision_free:
n_attempt += 1
print('gripper has collision')
continue
#self.episode_metrics['arm_ik_time'] += time() - ik_start
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, True)
p.restoreState(state_id)
p.removeState(state_id)
return arm_joint_positions
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, True)
p.restoreState(state_id)
p.removeState(state_id)
#self.episode_metrics['arm_ik_time'] += time() - ik_start
return None
