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
