def inverse_kinematics_async(self,
callback,
errback,
frame,
base_link,
group,
joint_names,
joint_positions,
avoid_collisions=True,
constraints=None,
attempts=8,
attached_collision_meshes=None):
"""Asynchronous handler of MoveIt IK service."""
header = Header(frame_id=base_link)
pose = Pose.from_frame(frame)
pose_stamped = PoseStamped(header, pose)
joint_state = JointState(name=joint_names,
position=joint_positions,
header=header)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if attached_collision_meshes:
for acm in attached_collision_meshes:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
ik_request = PositionIKRequest(group_name=group,
robot_state=start_state,
constraints=constraints,
pose_stamped=pose_stamped,
avoid_collisions=avoid_collisions,
attempts=attempts)
self.GET_POSITION_IK(self, (ik_request, ), callback, errback)
def inverse_kinematics_async(self,
callback,
errback,
frame,
base_link,
group,
joint_names,
joint_positions,
avoid_collisions=True,
constraints=None,
attempts=8):
"""
"""
header = Header(frame_id=base_link)
pose = Pose.from_frame(frame)
pose_stamped = PoseStamped(header, pose)
joint_state = JointState(name=joint_names,
position=joint_positions,
header=header)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
ik_request = PositionIKRequest(group_name=group,
robot_state=start_state,
constraints=constraints,
pose_stamped=pose_stamped,
avoid_collisions=avoid_collisions,
attempts=attempts)
self.GET_POSITION_IK(self, (ik_request, ), callback, errback)
def inverse_kinematics_async(self,
callback,
errback,
frame_WCF,
start_configuration=None,
group=None,
options=None):
"""Asynchronous handler of MoveIt IK service."""
base_link = options['base_link']
header = Header(frame_id=base_link)
pose_stamped = PoseStamped(header, Pose.from_frame(frame_WCF))
joint_state = JointState(name=start_configuration.joint_names,
position=start_configuration.joint_values,
header=header)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if options.get('attached_collision_meshes'):
for acm in options['attached_collision_meshes']:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
constraints = convert_constraints_to_rosmsg(options.get('constraints'),
header)
timeout_in_secs = options.get('timeout', 2)
timeout_duration = Duration(timeout_in_secs, 0).to_data()
ik_request = PositionIKRequest(group_name=group,
robot_state=start_state,
constraints=constraints,
pose_stamped=pose_stamped,
avoid_collisions=options.get(
'avoid_collisions', True),
attempts=options.get('attempts', 8),
timeout=timeout_duration)
# The field `attempts` was removed in Noetic (and higher)
# so it needs to be removed from the message otherwise it causes a serialization error
# https://github.com/ros-planning/moveit/pull/1288
if self.ros_client.ros_distro not in (RosDistro.KINETIC,
RosDistro.MELODIC):
del ik_request.attempts
def convert_to_positions(response):
callback((response.solution.joint_state.position,
response.solution.joint_state.name))
self.GET_POSITION_IK(self.ros_client, (ik_request, ),
convert_to_positions, errback)
def inverse_kinematics_async(self,
callback,
errback,
robot,
frame,
group,
start_configuration,
avoid_collisions=True,
constraints=None,
attempts=8,
attached_collision_meshes=None):
"""Asynchronous handler of MoveIt IK service."""
base_link = robot.model.root.name
header = Header(frame_id=base_link)
pose = Pose.from_frame(frame)
pose_stamped = PoseStamped(header, pose)
joint_state = JointState(name=start_configuration.joint_names,
position=start_configuration.values,
header=header)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if attached_collision_meshes:
for acm in attached_collision_meshes:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
constraints = self._convert_constraints_to_rosmsg(constraints, header)
ik_request = PositionIKRequest(group_name=group,
robot_state=start_state,
constraints=constraints,
pose_stamped=pose_stamped,
avoid_collisions=avoid_collisions,
attempts=attempts)
def convert_to_positions(response):
callback((response.solution.joint_state.position,
response.solution.joint_state.name))
self.GET_POSITION_IK(self, (ik_request, ), convert_to_positions,
errback)
def inverse_kinematics_async(self,
callback,
errback,
frame_WCF,
start_configuration=None,
group=None,
options=None):
"""Asynchronous handler of MoveIt IK service."""
base_link = options['base_link']
header = Header(frame_id=base_link)
pose_stamped = PoseStamped(header, Pose.from_frame(frame_WCF))
joint_state = JointState(name=start_configuration.joint_names,
position=start_configuration.values,
header=header)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if options.get('attached_collision_meshes'):
for acm in options['attached_collision_meshes']:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
constraints = convert_constraints_to_rosmsg(options.get('constraints'),
header)
ik_request = PositionIKRequest(group_name=group,
robot_state=start_state,
constraints=constraints,
pose_stamped=pose_stamped,
avoid_collisions=options.get(
'avoid_collisions', True),
attempts=options.get('attempts', 8))
def convert_to_positions(response):
callback((response.solution.joint_state.position,
response.solution.joint_state.name))
self.GET_POSITION_IK(self.ros_client, (ik_request, ),
convert_to_positions, errback)