def plan(self, joints=None):
""" Return a tuple of the motion planning results such as
(success flag : boolean, trajectory message : RobotTrajectory,
planning time : float, error code : MoveitErrorCodes) """
if type(joints) is JointState:
self.set_joint_value_target(joints)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif joints is not None:
try:
self.set_joint_value_target(self.get_remembered_joint_values()[joints])
except MoveItCommanderException:
self.set_joint_value_target(joints)
(error_code_msg, trajectory_msg, planning_time) = self._g.plan()
error_code = MoveItErrorCodes()
error_code.deserialize(error_code_msg)
plan = RobotTrajectory()
return (error_code.val == MoveItErrorCodes.SUCCESS,
plan.deserialize(trajectory_msg),
planning_time,
error_code)
def plan(self, joints=None):
"""Return a tuple of the motion planning results such as
(success flag : boolean, trajectory message : RobotTrajectory,
planning time : float, error code : MoveitErrorCodes)"""
if type(joints) is JointState:
self.set_joint_value_target(joints)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif joints is not None:
try:
self.set_joint_value_target(
self.get_remembered_joint_values()[joints])
except MoveItCommanderException:
self.set_joint_value_target(joints)
(error_code_msg, trajectory_msg, planning_time) = self._g.plan()
error_code = MoveItErrorCodes()
error_code.deserialize(error_code_msg)
plan = RobotTrajectory()
return (
error_code.val == MoveItErrorCodes.SUCCESS,
plan.deserialize(trajectory_msg),
planning_time,
error_code,
)
def test_validation(self):
current = np.asarray(self.group.get_current_joint_values())
error_code1, plan1, time = self.plan(current + 0.2)
error_code2, plan2, time = self.plan(current + 0.2)
# both plans should have succeeded:
error_code = MoveItErrorCodes()
error_code.deserialize(error_code1)
self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS)
error_code = MoveItErrorCodes()
error_code.deserialize(error_code2)
self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS)
# first plan should execute
self.assertTrue(self.group.execute(plan1))
# second plan should be invalid now (due to modified start point) and rejected
self.assertFalse(self.group.execute(plan2))
# newly planned trajectory should execute again
error_code3, plan3, time = self.plan(current)
error_code = MoveItErrorCodes()
error_code.deserialize(error_code3)
self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS)
self.assertTrue(self.group.execute(plan3))
def test(self):
current = np.asarray(self.group.get_current_joint_values())
for i in range(30):
target = current + np.random.uniform(-0.5, 0.5, size=current.shape)
# if plan was successfully executed, current state should be reported at target
error_code1, plan, time = self.plan(target)
error_code = MoveItErrorCodes()
error_code.deserialize(error_code1)
if (error_code.val == MoveItErrorCodes.SUCCESS) and self.group.execute(plan):
actual = np.asarray(self.group.get_current_joint_values())
self.assertTrue(np.allclose(target, actual, atol=1e-4, rtol=0.0))
# otherwise current state should be still the same
else:
actual = np.asarray(self.group.get_current_joint_values())
self.assertTrue(np.allclose(current, actual, atol=1e-4, rtol=0.0))
def test(self):
current = np.asarray(self.group.get_current_joint_values())
for i in range(30):
target = current + np.random.uniform(-0.5, 0.5, size=current.shape)
# if plan was successfully executed, current state should be reported at target
error_code1, plan, time = self.plan(target)
error_code = MoveItErrorCodes()
error_code.deserialize(error_code1)
if (error_code.val
== MoveItErrorCodes.SUCCESS) and self.group.execute(plan):
actual = np.asarray(self.group.get_current_joint_values())
self.assertTrue(
np.allclose(target, actual, atol=1e-4, rtol=0.0))
# otherwise current state should be still the same
else:
actual = np.asarray(self.group.get_current_joint_values())
self.assertTrue(
np.allclose(current, actual, atol=1e-4, rtol=0.0))
def test_validation(self):
current = np.asarray(self.group.get_current_joint_values())
error_code1, plan1, time = self.plan(current + 0.2)
error_code2, plan2, time = self.plan(current + 0.2)
# both plans should have succeeded:
error_code = MoveItErrorCodes()
error_code.deserialize(error_code1)
self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS)
error_code = MoveItErrorCodes()
error_code.deserialize(error_code2)
self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS)
# first plan should execute
self.assertTrue(self.group.execute(plan1))
# second plan should be invalid now (due to modified start point) and rejected
self.assertFalse(self.group.execute(plan2))
# newly planned trajectory should execute again
error_code3, plan3, time = self.plan(current)
self.assertTrue(self.group.execute(plan3))
error_code = MoveItErrorCodes()
error_code.deserialize(error_code3)
self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS)
: boolean, trajectory message
: RobotTrajectory, planning time
: float, error code
: MoveitErrorCodes) ""
"
if type(joints) is JointState : self.set_joint_value_target(joints)
elif type(joints) is Pose : self.set_pose_target(joints)
elif joints is not None : try : self.set_joint_value_target(self.get_remembered_joint_values()[joints])
except MoveItCommanderException:
self.set_joint_value_target(joints)
(error_code_msg, trajectory_msg, planning_time) = self._g.plan()
error_code = MoveItErrorCodes()
error_code.deserialize(error_code_msg)
plan = RobotTrajectory()
return (error_code.val == MoveItErrorCodes.SUCCESS,
plan.deserialize(trajectory_msg),
planning_time,
error_code)
def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, algorithm, avoid_collisions=True, path_constraints=None):
""" Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
if path_constraints:
if type(path_constraints) is Constraints:
constraints_str = conversions.msg_to_string(path_constraints)
else:
raise MoveItCommanderException("Unable to set path constraints, unknown constraint type " + type(path_constraints))
(ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, algorithm, avoid_collisions, constraints_str)