def createHandGoal(side, j1, j2, j3):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1, j2 and j3 for the joint positions
with the hand specified in side
@arg side string 'right' or 'left'
@arg j1 float value for joint 'hand_'+side+'_thumb_joint'
@arg j2 float value for joint 'hand_'+side+'_middle_joint'
@arg j3 float value for joint 'hand_'+side+'_index_joint'
@return FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('hand_'+side+'_thumb_joint')
fjtg.trajectory.joint_names.append('hand_'+side+'_middle_joint')
fjtg.trajectory.joint_names.append('hand_'+side+'_index_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.positions.append(j3)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
fjtg.trajectory.points.append(point)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
def createHandGoal(side, j1, j2, j3):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1, j2 and j3 for the joint positions
with the hand specified in side
@arg side string 'right' or 'left'
@arg j1 float value for joint 'hand_'+side+'_thumb_joint'
@arg j2 float value for joint 'hand_'+side+'_middle_joint'
@arg j3 float value for joint 'hand_'+side+'_index_joint'
@return FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('hand_' + side + '_thumb_joint')
fjtg.trajectory.joint_names.append('hand_' + side + '_middle_joint')
fjtg.trajectory.joint_names.append('hand_' + side + '_index_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.positions.append(j3)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
fjtg.trajectory.points.append(point)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
def createHeadGoal(j1, j2):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1 and j2 for the joint positions
@arg j1 float value for head_1_joint
@arg j2 float value for head_2_joint
@returns FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('head_1_joint')
fjtg.trajectory.joint_names.append('head_2_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.points.append(point)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
def make_joint_tolerance(pos, name):
j = JointTolerance()
j.position = pos
j.velocity = 1
j.acceleration = 100 # this is high because if we bump the arm, the controller will abort unnecessarily
j.name = name
return j
def execute(self, userdata):
rospy.loginfo('In create_move_group_joints_goal')
_move_joint_goal = FollowJointTrajectoryGoal()
_move_joint_goal.trajectory.joint_names = userdata.move_joint_list
jointTrajectoryPoint = JointTrajectoryPoint()
jointTrajectoryPoint.positions = userdata.move_joint_poses
for x in range(0, len(userdata.move_joint_poses)):
jointTrajectoryPoint.velocities.append(0.0)
jointTrajectoryPoint.time_from_start = rospy.Duration(2.0)
_move_joint_goal.trajectory.points.append(jointTrajectoryPoint)
for joint in _move_joint_goal.trajectory.joint_names:
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
_move_joint_goal.goal_tolerance.append(goal_tol)
_move_joint_goal.goal_time_tolerance = rospy.Duration(2.0)
_move_joint_goal.trajectory.header.stamp = rospy.Time.now()
rospy.loginfo('Move_Joint_GOAL: ' + str(_move_joint_goal))
userdata.move_joint_goal = _move_joint_goal
return 'succeeded'
def execute(self,traj,speed=DEFAULT_SPEED,joint_tolerance=DEFAULT_JOINT_TOLERANCE,speed_tolerance=DEFAULT_SPEED_TOLERANCE,timeout=DEFAULT_TIMEOUT,path_tolerance=0):
traj = deepcopy(traj)
self.applyTrajectorySpeed(traj,speed)
rospy.logdebug("Performing trajectory on %s at speed %f tolerance %f speed_tolerance %f timeout %f"%(self.side,speed,joint_tolerance,speed_tolerance,timeout))
joint_names=traj.joint_trajectory.joint_names
self.waitForStopped()
with self.moving_lock:
if self.is_moving!=0:
rospy.logerr("ERROR: trajectory already running")
return False
self.is_moving=1
goal=FollowJointTrajectoryActionGoal()
self.current_goal_id+=1
goal.goal_id.id=self.side+"_"+str(self.current_goal_id)
goal.goal.trajectory=traj.joint_trajectory
if path_tolerance<=0: path_tolerance=DEFAULT_PATH_TOLERANCE*speed
for name in joint_names:
t=JointTolerance()
t.name=name
t.position=joint_tolerance
t.velocity=speed_tolerance
t.acceleration=0
goal.goal.goal_tolerance.append(t)
pt=JointTolerance()
pt.name=name
if name.find("w2") is False:
pt.position=path_tolerance
else:
pt.position=0.0
goal.goal.path_tolerance.append(pt)
goal.goal.goal_time_tolerance=rospy.Duration(timeout)
# Send goal and wait for trajectory finished
self.pub_traj_goal.publish(goal)
self.waitForStopped()
return not self.result_error
def create_position_goal(self, absolute, position_setpoint, velocity):
current_position = self.message.position
# set up variables
trajectory = JointTrajectory()
point = JointTrajectoryPoint()
# Final position
if (absolute == ActuatorsState.ABS):
# absolute move
point.positions = [position_setpoint]
else:
# Relative move
point.positions = [current_position + position_setpoint]
point.velocities = [0] # cubic spline in position, smooth moves
# point.accelerations = [0] # cubic spline in position, smooth moves
trajectory.points.append(point)
# How long to get there
if (velocity != 0):
time_for_move = abs(current_position -
position_setpoint) / velocity
else:
time_for_move = 0 # porbably should handle different
if (time_for_move <= 0.1):
time_for_move = 0.1 # probably asserting joint.
trajectory.points[0].time_from_start = rospy.Duration(time_for_move)
# specify which joint to move
trajectory.joint_names.append(self.message.name)
follow_trajectory_goal = FollowJointTrajectoryGoal()
follow_trajectory_goal.trajectory = trajectory
# TOLERANCES FOR MOVE
# For some reason the defaults are not being loaded from control.yaml
tolerance = JointTolerance()
tolerance.name = self.message.name # joint name
tolerance.velocity = -1 # no fault for velocity
tolerance.acceleration = -1 # no fault for acceleration
# Tracking fault
tolerance.position = -1
follow_trajectory_goal.path_tolerance.append(tolerance)
# End position fault
tolerance.position = -1 # p.constraints.goal_pos
follow_trajectory_goal.goal_tolerance.append(tolerance)
# Time to reach goal precision
follow_trajectory_goal.goal_time_tolerance = rospy.Duration(10.0)
return follow_trajectory_goal
def __init__(self, blackboard, _):
super(ExtendTorso, self).__init__(blackboard)
self.first_iteration = True
self.repeat = False
self.goal = FollowJointTrajectoryGoal()
torso_command = JointTrajectory()
torso_command.joint_names.append("torso_lift_joint")
jtp = JointTrajectoryPoint()
jtp.positions.append(0.35)
jtp.time_from_start = rospy.Duration.from_sec(2.0)
torso_command.points.append(jtp)
self.goal.trajectory = torso_command
jt = JointTolerance()
jt.name = 'torso_lift_joint'
jt.position = 0.01
self.goal.goal_tolerance.append(jt)
self.goal.goal_tolerance
p1.time_from_start = rospy.Duration.from_sec(3.0)
p2 = JointTrajectoryPoint()
p2.positions = [0.5, 0.5, 0, 0, 0, 0]
p2.velocities = [0, 0, 0, 0, 0, 0]
p2.time_from_start = rospy.Duration.from_sec(5.0)
p3 = JointTrajectoryPoint()
p3.positions = [0, 0, 0, 0, 0, 0]
p3.velocities = [0, 0, 0, 0, 0, 0]
p3.time_from_start = rospy.Duration.from_sec(7.0)
goal.trajectory.points = [p1, p2, p3]
pt1 = JointTolerance()
pt1.name = 'joint_1'
pt1.position = 10.1
pt1.velocity = 10.1
pt1.acceleration = 10.1
pt2 = JointTolerance()
pt2.name = 'joint_2'
pt2.position = 10.1
pt2.velocity = 10.1
pt2.acceleration = 10.1
pt3 = JointTolerance()
pt3.name = 'joint_3'
pt3.position = 10.1
pt3.velocity = 10.1
pt3.acceleration = 10.1
pt4 = JointTolerance()
pt4.name = 'joint_4'
def on_enter(self, userdata):
# Clear data
userdata.joint_tolerances = None
self.return_code = None
try:
num_names = len(userdata.joint_names)
num_posn = len(self.position_constraints)
num_vel = len(self.velocity_constraints)
num_acc = len(self.acceleration_constraints)
if (num_posn > 1 and num_posn != num_names):
Logger.logwarn(
'Mis-match in joint names and position constraints -%d vs. %d'
% (num_names, num_posn))
self.return_code = 'param_error'
if (num_vel > 1 and num_vel != num_names):
Logger.logwarn(
'Mis-match in joint names and position constraints - %d vs. %d'
% (num_names, num_vel))
self.return_code = 'param_error'
if (num_acc > 1 and num_acc != num_names):
Logger.logwarn(
'Mis-match in joint names and position constraints - %d vs. %d'
% (num_names, num_acc))
self.return_code = 'param_error'
if self.return_code is not None:
return self.return_code
tolerances = []
for ndx, name in enumerate(userdata.joint_names):
jt = JointTolerance()
jt.name = name
if (num_posn > 1):
jt.position = self.position_constraints[ndx]
else:
jt.position = self.position_constraints[0]
if (num_vel > 1):
jt.velocity = self.velocity_constraints[ndx]
else:
jt.velocity = self.velocity_constraints[0]
if (num_acc > 1):
jt.acceleration = self.acceleration_constraints[ndx]
else:
jt.acceleration = self.acceleration_constraints[0]
tolerances.append(jt)
# Successfully set the joint tolerances and added to user data
userdata.joint_tolerances = tolerances
self.return_code = 'configured'
except Exception as e:
Logger.logwarn('Unable to configure joint tolerances for %s \n%s' %
(self.name, str(e)))
self.return_code = 'param_error'
return
