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 main():
ioloop = asyncio.get_event_loop()
register_asyncio_shutdown_handler(ioloop)
# create instance of movegroup to provide motion services
move_group = MoveGroup()
try:
print('---------- wsg gripper -------------')
# create instance of wsg gripper by name
properties = WeissWsgGripperProperties('wsg50')
wsg_gripper = WeissWsgGripper(properties, move_group.motion_service)
print('homeing')
ioloop.run_until_complete(wsg_gripper.homing())
print('move gripper')
ioloop.run_until_complete(wsg_gripper.move(0.1, 0.05, 0.05, True))
print('perform grasp')
result = ioloop.run_until_complete(wsg_gripper.grasp(0.02, 0.1, 0.05))
print('---------- common gripper -------------')
# create instance of common gripper by name and driver rosnode name
properties1 = CommonGripperProperties('wsg50', 'xamla/wsg_driver')
gripper = CommonGripper(properties1, move_group.motion_service)
print('move')
result1 = ioloop.run_until_complete(gripper.move(0.1, 0.005))
finally:
ioloop.close()
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 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(poses: List[Pose], pre_place_jvs: List[JointValues], home: JointValues, move_group : MoveGroup) :
"""
This function does a pick and place motion to every pose
Parameters
----------
poses : List[Pose]
Defines the place poses to be addressed
pre_place_jvs : List[JointValues]
Defines the joint values of the pre place positions
home : JointValues
The joint values, to which the robot returns after place
move_group : MoveGroup
"""
loop = asyncio.get_event_loop()
# Register the loop handler to be shutdown appropriately
register_asyncio_shutdown_handler(loop)
async def place(jv_pre_place: JointValues, place: Pose) -> None:
"""
This asynchronous function lets the robot move from home to every place position and
back in a "pick and place" manner
The movement from pre place JointValues to place Pose and back is done by moving the end
effector in a linear fashion in cartesian space.
The movement is planned an executed on the fly.
Parameters
----------
jv_pre_place : JointValues
The pre place configuration
jv_place : JointValues
The place configuration
"""
end_effector = move_group.get_end_effector()
# Creates a joint path over the joint values to the target pose
joint_path = JointPath(home.joint_set, [home, jv_pre_place ])
# Move over the joints to target pose
await move_group.move_joints_collision_free(joint_path)
pre_place_pose = end_effector.compute_pose(jv_pre_place)
await end_effector.move_poses_linear(CartesianPath([pre_place_pose, place]),
velocity_scaling = 0.05)
# do something, e.g. place an object
await end_effector.move_poses_linear(CartesianPath([place, pre_place_pose]),
velocity_scaling = 0.05)
# Creates a joint path over the joint values to the home pose
joint_path_back = JointPath(home.joint_set, [jv_pre_place, home ])
# Move back over the joint path
await move_group.move_joints_collision_free(joint_path_back)
try:
# Move to home position
loop.run_until_complete(move_group.move_joints_collision_free(home))
# For every pose we want to address, do a pick and place
for i in range(len(pre_place_jvs)):
print("Placing element {}".format(i+1))
loop.run_until_complete(place(pre_place_jvs[i], poses[i]))
finally:
loop.close()
def main():
world_view_client = WorldViewClient()
input_var = input("Please type in the torch filename (with path) of the sensorhead stereo camera calibration (e.g. \"calibration/torso_cameras/stereo_cams_<serial1>_<serial2>.t7\" ): ")
cam_calib_fn = str(input_var)
stereocalib_sensorhead_cams = torchfile.load(cam_calib_fn)
end_of_right_serial = cam_calib_fn.rfind(".")
start_of_right_serial = cam_calib_fn.rfind("_")
right_serial = cam_calib_fn[start_of_right_serial+1:end_of_right_serial]
end_of_left_serial = start_of_right_serial
start_of_left_serial = cam_calib_fn.rfind("_", 0, end_of_left_serial)
left_serial = cam_calib_fn[start_of_left_serial+1:end_of_left_serial]
print("right_serial:")
print(right_serial)
print("left_serial:")
print(left_serial)
print("Please type in the exposure time of the cameras [in microseconds],")
input_var = input("or press \'enter\' to use 120000ms:")
if input_var == "" :
exposure_time = 120000
else :
exposure_time = int(input_var)
print("exposure_time:")
print(exposure_time)
print(type(exposure_time))
move_group_names = MotionService.query_available_move_groups()
move_group_name = move_group_names[2].name # left_arm_torso
flange_link_name = move_group_names[2].end_effector_link_names[0] # arm_left_link_tool0
print("For which arm will the tooltip be calibrated? Please type l or r, then press \'Enter\'.")
print("l: left arm")
print("r: right arm")
which = sys.stdin.read(1)
if which == "r" :
move_group_name = move_group_names[3].name # right_arm_torso
flange_link_name = move_group_names[3].end_effector_link_names[0] # arm_right_link_tool0
print("move_group_name:")
print(move_group_name)
move_group = MoveGroup(move_group_name)
print("flange_link_name")
print(flange_link_name)
torso_link_name = "torso_link_b1"
print("torso_link_name:")
print(torso_link_name)
last_slash = cam_calib_fn.rfind("/")
torso_to_cam_fn = cam_calib_fn[:last_slash] + "/LeftCam_torso_joint_b1.t7"
print("Please type in the name of the torchfile (with path) containing the transformation between torso joint and torso cameras,")
input_var = input("or press \'enter\' to use " + torso_to_cam_fn)
if input_var != "" :
torso_to_cam_fn = str(input_var)
print("torso_to_cam_fn:")
print(torso_to_cam_fn)
torso_to_cam_t7 = torchfile.load(torso_to_cam_fn)
torso_to_cam = Pose.from_transformation_matrix(matrix=torso_to_cam_t7, frame_id=torso_link_name, normalize_rotation=False)
pattern_localizer = patternlocalisation.PatternLocalisation()
pattern_localizer.circleFinderParams.minArea = 50
pattern_localizer.circleFinderParams.maxArea = 1000
pattern_localizer.setPatternData(8, 11, 0.003)
pattern_localizer.setStereoCalibration(stereocalib_sensorhead_cams)
#pattern_localizer.setFindCirclesGridFlag(cv.CALIB_CB_ASYMMETRIC_GRID) # Don't use "cv.CALIB_CB_CLUSTERING", because it's much more sensitive to background clutter.
pattern_localizer.setFindCirclesGridFlag(cv.CALIB_CB_ASYMMETRIC_GRID + cv.CALIB_CB_CLUSTERING)
world_view_folder = '/Calibration'
storage_folder = '/tmp/calibration/storage_tooltipcalib'
offline = False
tooltip_calib = TooltipCalibration(pattern_localizer, stereocalib_sensorhead_cams, left_serial, right_serial, exposure_time,
world_view_client, world_view_folder, move_group, torso_to_cam, storage_folder, torso_link_name,
flange_link_name, offline)
tooltip_calib.runTooltipCalib()
def get_move_group() -> MoveGroup:
return MoveGroup(get_move_group_name())
def get_full_body_move_group() -> MoveGroup:
return MoveGroup("/sda10f")
def get_left_move_group() -> MoveGroup:
return MoveGroup("/sda10f/sda10f_r1_controller")
def get_right_move_group() -> MoveGroup:
return MoveGroup("/sda10f/sda10f_r2_controller")
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()