def calculate_place_joint_values(poses: List[Pose],
pre_place_jvs: List[JointValues],
move_group: MoveGroup,
world_view_client: WorldViewClient) \
-> List[JointValues]:
"""
Calculates the place joint values
Since we want the robot to move from pre place to place pose, we use the
corresponding pre place joint values as seed for the place joint values.
Parameters
----------
poses : List[Pose]
A list of poses for which the joint values should be calculated
pre_place_jvs : List[JointValues]
The seeds for every pose
move_group : MoveGroup
world_view_client: WorldViewClient
Returns
------
List[JointValues]
A list of joint values for every pose
"""
end_effector = move_group.get_end_effector()
place_jvs = []
for i in range(len(pre_place_jvs)):
# The i-th pre place joint values are used for the i-th position as seed
joint_values = end_effector.inverse_kinematics(poses[i],
collision_check=True,
seed=pre_place_jvs[i])
place_jvs.append(joint_values)
return place_jvs
def calculate_pre_place_joint_values(pre_place_poses: List[Pose],
jv_home: JointValues,
move_group: MoveGroup,
world_view_client: WorldViewClient,
world_view_folder: str) \
-> List[JointValues]:
"""
Calculates the pre place joint values
Since we want the robot arm to go back and forth from the home configuration
to the poses on the grid, we use the home joint value as a const seed for
every pose to minimize the distance for the joints to make.
Calling inverse_kinematics_many with "const_seed = True" lets us exclusively
using the jv_home joint values as seed.
"const_seed = False" would use the previously calculated joint values as seed
for the next on when traversing pre_place_poses. This could be useful when
we went from pose to pose without visiting the jv_home configuration in between.
Parameters
----------
pre_place_poses : List[Pose]
A list of poses for which the joint values should be calculated
jv_home : JointValues
The seed to be used
move_group : MoveGroup
world_view_client: WorldViewClient
Returns
------
List[JointValues]
A list of joint values for every pose
"""
end_effector = move_group.get_end_effector()
try:
pre_place_jvs = example_02_3_create_joint_values_from_poses.main(
pre_place_poses,
jv_home,
end_effector)
except Exception as e:
print("The inverse kinematics operation could not be applied on all the positions.")
world_view_client.remove_element("generated", world_view_folder)
raise e
# export every calculated joint values to world view to illustrate the result
for i in range(len(pre_place_jvs)):
world_view_client.add_joint_values("joint_values_{}".format(str(i).zfill(2)),
"/{}/generated/pre_place_joint_values".format(world_view_folder),
pre_place_jvs[i])
return pre_place_jvs
def find_shifted_joint_values(joint_values: JointValues, diff_pose: Pose,
move_group: MoveGroup) -> JointValues:
"""
Find joint values for the shifted pose of the joint_values end effector, if
possible.
This function takes a translation vector and joint values and tries to find
corresponding joint values shifted by "diff_pose" in Cartesian Space
This is done by getting the current pose of the end effector joint values,
applying the translation and rotation of "diff_pose", and then applying
inverse kinematics to generate the joint values anew at the proposed poses.
Parameters
----------
joint_values: JointValues
The joint values to be shifted
diff_pose: Pose
The pose which defines translation an rotation
move_group: MoveGroup
Returns
----------
JointValues
The shifted jointValues
Exceptions
----------
ServiceException
Raised when the inverse kinematics operation was not successfull
"""
# Get the pose of the end effetcor
end_effector = move_group.get_end_effector()
old_pose = end_effector.compute_pose(joint_values)
# translate and rotate
newPose = old_pose.translate(diff_pose.translation).rotate(diff_pose.quaternion)
# calculate joint values at new pose, which might throw an exception if not
# possible
shifted_joint_values = end_effector.inverse_kinematics(newPose,
seed=joint_values, collision_check = True)
return shifted_joint_values
def main():
# create move group instance
move_group = MoveGroup()
# get default endeffector of the movegroup
end_effector = move_group.get_end_effector()
# create cartesian path and equivalent joint path
t1 = [0.502522, 0.2580, 0.3670]
q1 = Quaternion(w=0.304389, x=0.5272, y=0.68704, z=0.39666)
t2 = [0.23795, 0.46845, 0.44505]
q2 = Quaternion(w=0.212097, x=0.470916, y=0.720915, z=0.462096)
t3 = [0.6089578, 0.3406782, 0.208865]
q3 = Quaternion(w=0.231852, x=0.33222, y=0.746109, z=0.528387)
pose_1 = Pose(t1, q1)
pose_2 = Pose(t2, q2)
pose_3 = Pose(t3, q3)
cartesian_path = CartesianPath([pose_1, pose_2, pose_3])
plan = end_effector.move_cartesian(cartesian_path,
collision_check=True).plan()
stepped_client = plan.execute_supervised()
robot_chat = RobotChatClient()
robot_chat_stepped_motion = RobotChatSteppedMotion(robot_chat,
stepped_client,
move_group.name)
loop = asyncio.get_event_loop()
register_asyncio_shutdown_handler(loop)
try:
loop.run_until_complete(
robot_chat_stepped_motion.handle_stepwise_motions())
finally:
loop.close()
def plan_null_space_torso_move(
left_move_group: MoveGroup,
right_move_group: MoveGroup,
full_body_start: MoveGroup,
torso_joint_name: str,
goal_torso_angle: float) -> List[JointValues] :
"""
Calculates a list of JointValues, describing the null space torso move
Parameters
----------
left_move_group : MoveGroup
The MoveGroup of the left arm
right_move_group : MoveGroup
The MoveGroup of the right arm
full_body_start : MoveGroup
The The MoveGroup of the whole robot
torso_joint_name : str
The name of the torso joint
goal_torso_angle : float
The target torso angle the torso should reach
Returns
----------
List[JointValues]
The JointValues of the movement
"""
# Get the JointValues of the current left and right arm configuration
left_start = left_move_group.get_current_joint_positions()
right_start = right_move_group.get_current_joint_positions()
# Get the JointValues of the whole body
full_body = full_body_start.get_current_joint_positions()
# Get the end effectors of the left and right arm, respectively
left_end_effector = left_move_group.get_end_effector()
right_end_effector = right_move_group.get_end_effector()
# Calculate poses of the end effectors
left_start_pose = left_end_effector.compute_pose(left_start)
right_start_pose = right_end_effector.compute_pose(right_start)
# Get the current value of the torso joint
found, start_torso_angle = full_body.try_get_value(torso_joint_name)
assert(found)
# Define the amount of the JointValues configurations the movement should consist of
N = max(10, int(abs(goal_torso_angle - start_torso_angle)/math.radians(1)))
threshold = math.pi/4
waypoints = []
for i in range(N):
last_full_body = copy.deepcopy(full_body)
# Calculate the current angle between start and goal
alpha = (i - 1) / (N - 1)
angle = (1-alpha) * start_torso_angle + alpha * goal_torso_angle
print("Calculate index {} of {} with angle {}".format(i + 1, N, angle))
# Now we update the JointValues of the robot by adjusting the angle of
# the torso joint only
full_body = full_body.set_values(JointSet(torso_joint_name), [angle])
# Calculate the inverse kinematics for the left arm and give the
# altered "full_body" object (with updated angle) as seed.
# Since the move group for the left arm does not contain the joint
# of the torso, it can only update the joint of the arm and assumes
# the torso joint given by the seed to be fixed
left_tmp_jv = left_end_effector.inverse_kinematics(
pose=left_start_pose,
collision_check=True,
seed=full_body)
# Update the JointValues of the result of IK of the left arm
full_body = full_body.set_values(left_tmp_jv.joint_set, left_tmp_jv.values)
# Do the above for the right arm
right_tmp_jv = right_end_effector.inverse_kinematics(
pose=right_start_pose,
collision_check=True,
seed=full_body)
full_body = full_body.set_values(right_tmp_jv.joint_set, right_tmp_jv.values)
# Assert there were no IK jumps
# assert(2 > full_body.ik_jump_threshold detectIKJump(full_body, last_full_body, threshold)) #, 'jump IK detected')
waypoints.append(full_body)
return waypoints
def main():
# create move group instance
move_group = MoveGroup()
# get default endeffector of the movegroup
end_effector = move_group.get_end_effector()
# create cartesian path and equivalent joint path
t1 = [0.502522, 0.2580, 0.3670]
q1 = Quaternion(w=0.304389, x=0.5272, y=0.68704, z=0.39666)
t2 = [0.23795, 0.46845, 0.44505]
q2 = Quaternion(w=0.212097, x=0.470916, y=0.720915, z=0.462096)
t3 = [0.6089578, 0.3406782, 0.208865]
q3 = Quaternion(w=0.231852, x=0.33222, y=0.746109, z=0.528387)
pose_1 = Pose(t1, q1)
pose_2 = Pose(t2, q2)
pose_3 = Pose(t3, q3)
cartesian_path = CartesianPath([pose_1, pose_2, pose_3])
joint_path = end_effector.inverse_kinematics_many(cartesian_path,
False).path
loop = asyncio.get_event_loop()
register_asyncio_shutdown_handler(loop)
async def example_moves():
print('test MoveGroup class')
print('---------------- move joints -------------------')
await move_group.move_joints(joint_path)
print('---------------- move joints collision free -------------------')
await move_group.move_joints_collision_free(joint_path)
print('---------------- move joints supervised -------------------')
stepped_motion_client = move_group.move_joints_supervised(
joint_path, 0.5)
await run_supervised(stepped_motion_client)
print('----------------move joints collision free supervised -------------------')
stepped_motion_client = move_group.move_joints_collision_free_supervised(
joint_path, 0.5)
await run_supervised(stepped_motion_client)
print('test EndEffector class')
print('---------------- move poses -------------------')
await end_effector.move_poses(cartesian_path)
print('---------------- move poses collision free -------------------')
await end_effector.move_poses_collision_free(cartesian_path)
print('---------------- move poses supervised -------------------')
stepped_motion_client = end_effector.move_poses_supervised(
cartesian_path, None, 0.5)
await run_supervised(stepped_motion_client)
print('---------------- move poses collision free supervised -------------------')
stepped_motion_client = end_effector.move_poses_collision_free_supervised(
cartesian_path, None, 0.5)
await run_supervised(stepped_motion_client)
try:
loop.run_until_complete(example_moves())
finally:
loop.close()