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 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 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
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' pt4.position = 10.1 pt4.velocity = 10.1 pt4.acceleration = 10.1
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