def execute_JointTrajectory(self, joint_trajectory, wait=True):
"""Executes a trajectory. It does not check if the trajectory is safe, nor it performs
any interpolation or smoothing! For a more fine grained control use execute_trajectory.
Parameters:
joint_trajectory: a JointTrajectory msg
wait: block until trajectory is completed
"""
isinstance(joint_trajectory, JointTrajectory)
if joint_trajectory.joint_names[0][0] == "l":
client = self.l_arm_client
self.l_arm_done = False
done_cb = self.__l_arm_done_cb
else:
client = self.r_arm_client
self.r_arm_done = False
done_cb = self.__r_arm_done_cb
goal = JointTrajectoryGoal()
goal.trajectory = joint_trajectory
client.send_goal(goal, done_cb=done_cb)
if wait:
client.wait_for_result()
def __create_spin_command(self, arm, theta=math.pi):
if arm == "l":
jnts = self.robot_state.left_arm_positions
if arm == "r":
jnts = self.robot_state.right_arm_positions
command = JointTrajectory()
command.joint_names = [
"%s_shoulder_pan_joint" % arm[0],
"%s_shoulder_lift_joint" % arm[0],
"%s_upper_arm_roll_joint" % arm[0],
"%s_elbow_flex_joint" % arm[0],
"%s_forearm_roll_joint" % arm[0],
"%s_wrist_flex_joint" % arm[0],
"%s_wrist_roll_joint" % arm[0],
]
jnts[-1] += theta
command.points.append(
JointTrajectoryPoint(
positions=jnts,
velocities=[0.0] * (len(command.joint_names)),
accelerations=[],
time_from_start=rospy.Duration(0.1),
)
)
goal = JointTrajectoryGoal()
goal.trajectory = command
return goal
def set_arm_state(self, jvals, arm, wait=False):
""" Sets goal for indicated arm (r_arm/l_arm) using provided joint values"""
# Build trajectory message
if len(jvals) == 0:
rospy.logwarn("No %s_arm goal given" % arm[0])
if arm[0] == "l":
self.l_arm_done = True
elif arm[0] == "r":
self.r_arm_done = True
return
# rospy.loginfo("Senging %s_joints: %s" %(arm[0], str(jvals)))
command = JointTrajectory()
command.joint_names = [
"%s_shoulder_pan_joint" % arm[0],
"%s_shoulder_lift_joint" % arm[0],
"%s_upper_arm_roll_joint" % arm[0],
"%s_elbow_flex_joint" % arm[0],
"%s_forearm_roll_joint" % arm[0],
"%s_wrist_flex_joint" % arm[0],
"%s_wrist_roll_joint" % arm[0],
]
if arm[0] == "l":
client = self.l_arm_client
self.l_arm_done = False
elif arm[0] == "r":
client = self.r_arm_client
self.r_arm_done = False
command.points.append(
JointTrajectoryPoint(
positions=jvals,
velocities=[0] * (len(command.joint_names)),
accelerations=[],
time_from_start=rospy.Duration(self.time_to_reach),
)
)
# command.header.stamp = rospy.Time.now()
goal = JointTrajectoryGoal()
goal.trajectory = command
# rospy.loginfo("Sending command to %s" % arm)
if arm[0] == "l":
client.send_goal(goal, done_cb=self.__l_arm_done_cb)
elif arm[0] == "r":
client.send_goal(goal, done_cb=self.__r_arm_done_cb)
if wait:
client.wait_for_result()
def execute_trajectory(self, trajectory, times, vels, arm, wait=False):
"""
Executes a trajectory. It does not check if the trajectory is safe, nor it performs
any interpolation or smoothing! times and vels can be obtained from ik_utils with the method
@trajectory_times_and_vels
@param trajectory: a list of lists of joints
@param times: a list a times for each element in the trajectory.
@param vels: a list of velocities for each element in the trajectory
@param arm: either "left" or "right"
@param wait: if this call should block until the trajectory is over
"""
command = JointTrajectory()
command.header.stamp = rospy.get_rostime() + rospy.Duration(0.05)
command.joint_names = [
"%s_shoulder_pan_joint" % arm[0],
"%s_shoulder_lift_joint" % arm[0],
"%s_upper_arm_roll_joint" % arm[0],
"%s_elbow_flex_joint" % arm[0],
"%s_forearm_roll_joint" % arm[0],
"%s_wrist_flex_joint" % arm[0],
"%s_wrist_roll_joint" % arm[0],
]
if arm[0] == "l":
client = self.l_arm_client
self.l_arm_done = False
elif arm[0] == "r":
client = self.r_arm_client
self.r_arm_done = False
for jvals, t, v in zip(trajectory, times, vels):
command.points.append(
JointTrajectoryPoint(
positions=jvals, velocities=v, accelerations=[] * 7, time_from_start=rospy.Duration(t)
)
)
command.header.stamp = rospy.Time.now()
goal = JointTrajectoryGoal()
goal.trajectory = command
if arm[0] == "l":
client.send_goal(goal, done_cb=self.__l_arm_done_cb)
elif arm[0] == "r":
client.send_goal(goal, done_cb=self.__r_arm_done_cb)
if wait:
client.wait_for_result()
