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()
def move_to_joints(self,
side_prefix,
positions,
time_to_joint,
wait=False):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
if side_prefix == 'r':
traj_goal.trajectory.joint_names = self.r_joint_names
action_client = self.r_traj_action_client
else:
traj_goal.trajectory.joint_names = self.l_joint_names
action_client = self.l_traj_action_client
action_client.send_goal(traj_goal)
if wait:
time.sleep(time_to_joint)
def move_to_joints(self, positions, time_to_joint):
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
if (self.side_prefix == 'r'):
traj_goal.trajectory.joint_names = IK.r_joint_names
else:
traj_goal.trajectory.joint_names = IK.l_joint_names
self.traj_action_client.send_goal(traj_goal)
def knock(self):
for position in WaterPulse.positions:
velocities = [0] * len(position)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
time_to_joint = 2.0
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=position,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
result = 0
while (result < 2): # ACTIVE or PENDING
self.l_traj_action_client.wait_for_result()
result = self.l_traj_action_client.get_result()
def move_to_pose_cb(self, feedback):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
'''Moves the arm to the desired joints'''
print feedback
time_to_joint = 2.0
positions = self.ik.get_ik_for_ee(feedback.pose)
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
if (self.side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
pass