def move_head(self, pan , tilt): # Which joints define the head? head_joints = ['head_pan_joint', 'head_tilt_mod_joint'] # Create a single-point head trajectory with the head_goal as the end-point head_trajectory = JointTrajectory() head_trajectory.joint_names = head_joints head_trajectory.points.append(JointTrajectoryPoint()) head_trajectory.points[0].positions = pan , tilt head_trajectory.points[0].velocities = [0.0 for i in head_joints] head_trajectory.points[0].accelerations = [0.0 for i in head_joints] head_trajectory.points[0].time_from_start = rospy.Duration(3.0) # Send the trajectory to the head action server rospy.loginfo('Moving the head to goal position...') head_goal = FollowJointTrajectoryGoal() head_goal.trajectory = head_trajectory head_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal self.head_client.send_goal(head_goal) # Wait for up to 5 seconds for the motion to complete self.head_client.wait_for_result(rospy.Duration(2.0)) rospy.loginfo('...done')
def __init__(self): rospy.init_node('arm_simple_trajectory') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Which joints define the arm? arm_joints = ['joint_lift', 'joint_waist', 'joint_big_arm', 'joint_forearm', 'joint_wrist_pitching', 'joint_wrist_rotation'] if reset: # Set the arm back to the resting position arm_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] else: # Set a goal configuration for the arm arm_goal = [1.0, 1.0, 1.0, 1.0, 0.0, 0.0] # Connect to the right arm trajectory action server rospy.loginfo('Waiting for xm arm trajectory controller...') arm_client = actionlib.SimpleActionClient('xm_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) arm_client.wait_for_server() rospy.loginfo('...connected.') rospy.sleep(1) # Create a single-point arm trajectory with the arm_goal as the end-point arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].velocities = [0.0 for i in arm_joints] arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(3) # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') rospy.sleep(1) # Create an empty trajectory goal arm_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above arm_goal.trajectory = arm_trajectory # Specify zero tolerance for the execution time arm_goal.goal_time_tolerance = rospy.Duration(0) # Send the goal to the action server arm_client.send_goal(arm_goal) rospy.loginfo('...done') rospy.sleep(1)
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 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 move_to(self, result_trajectory, duration=5.0): trajectory = result_trajectory follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = trajectory self.client.send_goal(follow_goal) self.client.wait_for_result()
def send_arm_goal(self, arm_goal): arm_joint_names = ['joint_lift', 'joint_waist', 'joint_big_arm', 'joint_forearm', 'joint_wrist_pitching', 'joint_wrist_rotation'] rospy.loginfo("Waiting for follow_joint_trajectory server") self.arm_client.wait_for_server() rospy.loginfo("Connected to follow_joint_trajectory server") rospy.sleep(1) arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joint_names arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].time_from_start = rospy.Duration(10) rospy.loginfo("Preparing for moving the arm to goal position!") rospy.sleep(1) arm_goal_pos = FollowJointTrajectoryGoal() arm_goal_pos.trajectory = arm_trajectory arm_goal_pos.goal_time_tolerance = rospy.Duration(0) self.arm_client.send_goal(arm_goal_pos) rospy.loginfo("Send goal to the trajectory server successfully!") self.arm_client.wait_for_result()
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 execute_trajectory(self, trajectory, duration=5.0): follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = trajectory self.client.send_goal(follow_goal) #rospy.sleep(1.5) #self.client.cancel_goal() self.client.wait_for_result() self.finish_time = rospy.Time.now() self.believed_state = trajectory.points[-1].positions
def __init__(self): rospy.init_node('trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Which joints define the arm? arm_joints = ['link_1_joint', 'servo_2_joint', 'link_3_joint', 'link_4_joint', 'link_5_joint'] if reset: # Set the arm back to the resting position arm_goal = [0, 0, 0, 0, 0] else: # Set a goal configuration for the arm arm_goal = [-0.525487, 0.904972, 0.480017, -1.331166, 0.731413] # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') arm_client = actionlib.SimpleActionClient('right_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) arm_client.wait_for_server() rospy.loginfo('...connected.') # Create a single-point arm trajectory with the arm_goal as the end-point arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].velocities = [0.0 for i in arm_joints] arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(3.0) # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') # Create an empty trajectory goal arm_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above arm_goal.trajectory = arm_trajectory # Specify zero tolerance for the execution time arm_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal to the action server arm_client.send_goal(arm_goal) rospy.loginfo('...done')
def move_to(self, positions, duration=5.0): print "============= Moving" trajectory = JointTrajectory() trajectory.joint_names = self.joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = positions trajectory.points[0].velocities = [0.0 for _ in positions] trajectory.points[0].accelerations = [0.0 for _ in positions] trajectory.points[0].time_from_start = rospy.Duration(duration) follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = trajectory
def build_follow_trajectory(self, traj): # Build 'Follow Joint Trajectory' goal from trajectory (includes tolerances for error) traj_goal = FollowJointTrajectoryGoal() traj_goal.trajectory = traj tolerance = JointTolerance() tolerance.position = 0.05 tolerance.velocity = 0.1 traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))] traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))] traj_goal.goal_time_tolerance = rospy.Duration(3) return traj_goal
def btnPlay(self): if not self.ui.chkAllMotors.isChecked(): print("all motors should have torque") return if self.ui.spinSetFrame.value()>self.ui.spinPlayTo.value() or self.ui.spinSetFrame.value()<1 or self.ui.spinPlayTo.value()>self.totalFrames: print("\nwrong range in play\n") return trajectories=[] for cat in self.motorTrajectoryTopics: jointTraj=JointTrajectory() jts_names=[] for name in self.names: #if self.motorNamesAndCat[name]==cat and name!='l_hip_yaw': if self.motorNamesAndCat[name]==cat: jointTraj.joint_names.append(name+"_joint") jts_names.append(name) fcount=self.ui.spinPlayTo.value()-self.ui.spinSetFrame.value()+1 totalDelay=0.0 for n in range(0,fcount): pos_arr=JointTrajectoryPoint() readLoc=n+self.ui.spinSetFrame.value() delaySecs=float(self.TimeDelayFromPrevious[readLoc-1])/1000.0 totalDelay=totalDelay+delaySecs pos_arr.time_from_start=rospy.Duration().from_sec(totalDelay) for name in jts_names: pos_arr.positions.append(self.TrajectoryInfo[name][readLoc-1]) print("\n "+name+": "+str(self.TrajectoryInfo[name][readLoc-1])) velocity=1.0#2*pi rad/sec if n!=0: velocity=float(self.TrajectoryInfo[name][readLoc-1]-self.TrajectoryInfo[name][readLoc-2])/delaySecs ##pos_arr.velocities.append(velocity) jointTraj.points.append(pos_arr) jointTraj.header.stamp=rospy.Time.now() trajectories.append(jointTraj) trajClients=[] mm=0 for cat in self.motorTrajectoryTopics: trajClients.append(actionlib.SimpleActionClient(self.motorTrajectoryTopics[cat],FollowJointTrajectoryAction)) trajClients[mm].wait_for_server() mm=mm+1 #trajectories[nn] .. how to send this goal nn=0 for traj in trajClients: goal=FollowJointTrajectoryGoal() #goal.trajectory.points=trajectories[nn].points goal.trajectory=trajectories[nn] traj.send_goal(goal) nn=nn+1 print ("\nDone Sending Play Info...\n")
def move_to_start(self): self.moving_mode = True # Look down goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = '/base_link' goal.target.point.x = 1.5 goal.target.point.y = 0.0 goal.target.point.z = -0.2 goal.min_duration = rospy.Duration(0.5) if 0: logger.info('Point head to %s...', goal); self.head_point_client.send_goal(goal) if 0: logger.info('Point head sent') goal = GripperCommandGoal() goal.command.max_effort = 60 goal.command.position = 0.1 self.gripper_client.send_goal(goal) joints = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint', ] pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0] result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True) if result.error_code.val == MoveItErrorCodes.SUCCESS: if 0: logger.info('Got trajectory %s', result.planned_trajectory) follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = result.planned_trajectory.joint_trajectory logger.info('sending trajectory to arm...') self.arm_trajectory_client.send_goal(follow_goal) result = self.arm_trajectory_client.wait_for_result() logger.info('arm followed trajectory %s', result) else: logger.warn('moveToJointPosition returned %s', result) return result = self.head_point_client.wait_for_result() logger.info('head followed trajectory %s', result) logger.info('sending empty arm goal') empty_goal = FollowJointTrajectoryGoal() self.arm_trajectory_client.send_goal(empty_goal) logger.info('Point head done') self.moving_mode = False
def execute(self, trajectory, test=None, epsilon=0.1): """ Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz) This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected Non-blocking needs should deal with the JointTrajectory action server :param trajectory: either a RobotTrajectory or a JointTrajectory :param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz! :param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality :return: True if execution ended successfully, False otherwise """ def distance_to_first_point(point): joint_pos = np.array(point.positions) return np.linalg.norm(current_array - joint_pos) self.display(trajectory) with self._stop_lock: self._stop_reason = '' if isinstance(trajectory, RobotTrajectory): trajectory = trajectory.joint_trajectory elif not isinstance(trajectory, JointTrajectory): raise TypeError("Execute only accept RobotTrajectory or JointTrajectory") ftg = FollowJointTrajectoryGoal() ftg.trajectory = trajectory # check if it is necessary to move to the first point current = self.get_current_state() current_array = np.array([current.joint_state.position[current.joint_state.name.index(joint)] for joint in trajectory.joint_names]) if distance_to_first_point(trajectory.points[0]) > epsilon: # convert first point to robot state rs = RobotState() rs.joint_state.name = trajectory.joint_names rs.joint_state.position = trajectory.points[0].positions # move to the first point self.move_to_controlled(rs) # execute the input trajectory self.client.send_goal(ftg) # Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory while self.client.simple_state != SimpleGoalState.DONE: if callable(test) and test(): self.client.cancel_goal() return True if self._stop_reason!='': self.client.cancel_goal() return False self._rate.sleep() return True
def __init__(self): rospy.init_node("gripper_trajectory_demo") # Set to True to move back to the starting configurations reset = rospy.get_param("~reset", False) # Set the duration of the trajectory in seconds duration = rospy.get_param("~duration", 2.0) # Which joints define the head? gripper_joints = ["right_gripper_finger_joint"] # Set a goal configuration for the head if reset: gripper_goal = [0] else: gripper_goal = [0.25] # Connect to the head trajectory action server rospy.loginfo("Waiting for gripper trajectory controller...") gripper_client = actionlib.SimpleActionClient( "right_gripper_controller/follow_joint_trajectory", FollowJointTrajectoryAction ) gripper_client.wait_for_server() rospy.loginfo("...connected.") # Create a head trajectory with a single end point using the gripper_goal positions trajectory = JointTrajectory() trajectory.joint_names = gripper_joints trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = gripper_goal trajectory.points[0].velocities = [0.0 for i in gripper_joints] trajectory.points[0].accelerations = [0.0 for i in gripper_joints] trajectory.points[0].time_from_start = rospy.Duration(duration) # Send the trajectory to the head action server rospy.loginfo("Moving the gripper to goal position...") goal = FollowJointTrajectoryGoal() goal.trajectory = trajectory goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal gripper_client.send_goal(goal) # Wait for up to 5 seconds for the motion to complete gripper_client.wait_for_result(rospy.Duration(5.0)) rospy.loginfo("...done")
def get_hand_goal(goal_positions): grasp_msg = FollowJointTrajectoryGoal() point1 = JointTrajectoryPoint( positions=goal_positions, velocities=[0.0, 0.0, 0.0], time_from_start = rospy.Duration(1)) grasp_msg.trajectory = JointTrajectory( joint_names = ['hand_right_thumb_joint', 'hand_right_index_1_joint', 'hand_right_middle_1_joint'], points = [point1]) return grasp_msg
def on_enter(self, userdata): self._failed = False # create goal goal = FollowJointTrajectoryGoal() goal.trajectory = userdata.joint_trajectory goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2) # execute the motion try: self._client.send_goal(self._action_topic, goal) except Exception as e: Logger.logwarn('Was unable to execute joint trajectory:\n%s' % str(e)) self._failed = True
def __init__(self): rospy.init_node('head_trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set the duration of the trajectory in seconds duration = rospy.get_param('~duration', 3.0) # Which joints define the head? head_joints = ['head_pan_joint', 'head_tilt_joint'] # Set a goal configuration for the head if reset: head_goal = [0, 0] else: head_goal = [-1.3, -0.3] # Connect to the head trajectory action server rospy.loginfo('Waiting for head trajectory controller...') head_client = actionlib.SimpleActionClient('head_controller/follow_joint_trajectory', FollowJointTrajectoryAction) head_client.wait_for_server() rospy.loginfo('...connected.') # Create a head trajectory with a single end point using the head_goal positions trajectory = JointTrajectory() trajectory.joint_names = head_joints trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = head_goal trajectory.points[0].velocities = [0.0 for i in head_joints] trajectory.points[0].accelerations = [0.0 for i in head_joints] trajectory.points[0].time_from_start = rospy.Duration(duration) # Send the trajectory to the head action server rospy.loginfo('Moving the head to goal position...') goal = FollowJointTrajectoryGoal() goal.trajectory = trajectory goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal head_client.send_goal(goal) # Wait for up to 5 seconds for the motion to complete head_client.wait_for_result(rospy.Duration(5.0)) rospy.loginfo('...done')
def execute_traj_moveit(self, waypoints): # Cycle through waypoints for point in waypoints: plannedTraj = self.arm_planner.plan_jointTargetInput(point) if plannedTraj == None or len(plannedTraj.joint_trajectory.points) < 1: print "Error: no plan found" return -1 else: traj_goal = FollowJointTrajectoryGoal() traj_goal.trajectory = plannedTraj.joint_trajectory self.smooth_joint_trajectory_client.send_goal(traj_goal) self.smooth_joint_trajectory_client.wait_for_result() self.smooth_joint_trajectory_client.get_result() return 1
def move_to(self, positions, duration=5.0): if len(self.joint_names) != len(positions): print("Invalid trajectory position") return False trajectory = JointTrajectory() trajectory.joint_names = self.joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = positions trajectory.points[0].velocities = [0.0 for _ in positions] trajectory.points[0].accelerations = [0.0 for _ in positions] trajectory.points[0].time_from_start = rospy.Duration(duration) follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = trajectory self.client.send_goal(follow_goal) self.client.wait_for_result()
def move_joint(angles): goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = ['arm_base_joint', 'shoulder_joint','bottom_wrist_joint' ,'elbow_joint', 'top_wrist_joint'] point = JointTrajectoryPoint() point.positions = angles point.time_from_start = rospy.Duration(3) goal.trajectory.points.append(point) client.send_goal_and_wait(goal)
def move_to(self, positions, duration=5.0): if len(self.joint_names) != len(positions): print("Invalid position goal: got %d positions, can only handle one." %len(positions)) return False trajectory = JointTrajectory() trajectory.joint_names = self.joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = positions trajectory.points[0].velocities = [0.0 for _ in positions] trajectory.points[0].accelerations = [0.0 for _ in positions] trajectory.points[0].time_from_start = rospy.Duration(duration) follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = trajectory self.client.send_goal(follow_goal) self.client.wait_for_result()
def createTraj(self, point): self.joints_position = [] self.names =["joint1", "joint2", "joint3", "joint4", "gripper"] dur = rospy.Duration(1) pointTraj = JointTrajectoryPoint(positions=point, velocities=[0.5]*self.Joints, time_from_start=dur) self.joints_position.append(pointTraj) self.TrajPoint = JointTrajectory(joint_names=self.names, points=self.joints_position) self.TrajGoal = FollowJointTrajectoryGoal(trajectory=self.TrajPoint, goal_time_tolerance=rospy.Duration(3))
def _create_goal(self, joints_position, duration=2): goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = self._joint_names goal.trajectory.points = [JointTrajectoryPoint()] goal.trajectory.points[0].positions = joints_position goal.trajectory.points[0].velocities = [0.0] * len(self._joint_names) goal.trajectory.points[0].time_from_start = rospy.Duration(duration) return goal
def move_joint(self, angles): goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = ['shoulder_1_joint', 'shoulder_2_joint'] point = JointTrajectoryPoint() point.positions = angles point.time_from_start = rospy.Duration(3) goal.trajectory.points.append(point) self.jta.send_goal_and_wait(goal)
def move_to_point(self, point, joint_names=[''], interval=0.8): goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = self.joint_names point.time_from_start = rospy.Duration(interval) goal.trajectory.points.append(point) rospy.loginfo('Sending new goal for {}'.format(self.name)) self.jta_client.send_goal(goal) self.clear()
def move_to_initial(self, positions, duration): assert(len(self.joint_names) == len(positions)) trajectory = JointTrajectory() trajectory.joint_names = self.joint_names p = JointTrajectoryPoint() p.positions = positions p.time_from_start = rospy.Duration(duration) trajectory.points.append(p) follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = trajectory self.client.send_goal(follow_goal) self.client.wait_for_result()
def move_arm_joints(client, arm_joint_positions, duration=5.0): trajectory = JointTrajectory() trajectory.joint_names = ARM_JOINT_NAMES trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = arm_joint_positions trajectory.points[0].velocities = [0.0] * len(arm_joint_positions) trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions) trajectory.points[0].time_from_start = rospy.Duration(duration) arm_goal = FollowJointTrajectoryGoal() arm_goal.trajectory = trajectory arm_goal.goal_time_tolerance = rospy.Duration(0.0) print("Moving amr to joints: ", arm_joint_positions) client.send_goal(arm_goal) client.wait_for_result(rospy.Duration(duration + 1.0)) rospy.loginfo("...done")
def move_head_joints(client, head_joint_positions): trajectory = JointTrajectory() trajectory.joint_names = HEAD_JOINT_NAMES trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = head_joint_positions trajectory.points[0].velocities = [0.0] * len(head_joint_positions) trajectory.points[0].accelerations = [0.0] * len(head_joint_positions) trajectory.points[0].time_from_start = rospy.Duration(1.0) head_goal = FollowJointTrajectoryGoal() head_goal.trajectory = trajectory head_goal.goal_time_tolerance = rospy.Duration(0.0) print("Moving head to joints: ", head_joint_positions) client.send_goal(head_goal) client.wait_for_result(rospy.Duration(1.0)) rospy.loginfo("...done")
def __init__(self): rospy.init_node('trajectory_demo') # Which joints define the arm? arm_joints = ['x_joint'] # Set the arm back to the resting position arm_goal = [2] # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') arm_client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction) arm_client.wait_for_server() rospy.loginfo('...connected.') # Create a single-point arm trajectory with the arm_goal as the end-point arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].velocities = [0.0] arm_trajectory.points[0].accelerations = [0.0] arm_trajectory.points[0].time_from_start = rospy.Duration(3.0) # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') # Create an empty trajectory goal arm_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above arm_goal.trajectory = arm_trajectory # Specify zero tolerance for the execution time arm_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal to the action server arm_client.send_goal(arm_goal) # Wait for up to 5 seconds for the motion to complete arm_client.wait_for_result(rospy.Duration(5.0)) rospy.loginfo('...done')
def send_traj_(self, trajectory): print "Sending trajectory" #prepare goal trajgoal = FollowJointTrajectoryGoal() trajgoal.trajectory = trajectory # send goal self.joint_spline_trajectory_actionclient_.send_goal(trajgoal) # wait for result up to 30 seconds self.joint_spline_trajectory_actionclient_.wait_for_result(timeout=rospy.Duration.from_sec(50)) # analyze result joint_spline_trajectory_result_ = self.joint_spline_trajectory_actionclient_.get_result() if self.joint_spline_trajectory_actionclient_.get_state() != GoalStatus.SUCCEEDED: rospy.logerr("The joint_trajectory action has failed: " + str(joint_spline_trajectory_result_.error_code) ) return -1 else: rospy.loginfo("The joint_trajectory action has succeeded") return 0
def redundancy_resolution(q_i_change, q_index, alpha_scale, action_client): desired_twist = np.array([0, 0, 0, 0, 0, 0]) startingPoint = JointTrajectoryPoint() startingPoint.positions = arm_group.get_current_joint_values() startingPoint.time_from_start.secs = 0 q_i_desired = startingPoint.positions[q_index-1] + q_i_change trajectory = JointTrajectory() trajectory.points = [startingPoint] # parameters N_points = 300 Time_total_sec = 5.0 dt_point2point = Time_total_sec/N_points time_from_start = 0.0 for i in range(1, N_points): point = JointTrajectoryPoint() # compute null space projection jacobian = arm_group.get_jacobian_matrix(trajectory.points[i-1].positions) pseudoInverseJacobian = np.linalg.pinv(jacobian) # this mat is 7 by 6 nullSpaceProjector = \ np.identity(7) - pseudoInverseJacobian.dot(jacobian) # this mat is 7 by 7, Equation 5.1, I - J+ J q_i_current = trajectory.points[i-1].positions[q_index-1] gradient_q = np.zeros(7) gradient_q[q_index-1] = q_i_current-q_i_desired eta = alpha_scale*gradient_q # this vector is 7 by 1, Notes in Asana jointVelocities = \ pseudoInverseJacobian.dot(desired_twist) \ + nullSpaceProjector.dot(eta) # add the desired joint positions to the queue point.positions = \ (trajectory.points[i-1].positions \ + (jointVelocities*dt_point2point)).tolist() time_from_start = time_from_start + dt_point2point point.time_from_start = rospy.Duration.from_sec(time_from_start) trajectory.points.append(point) trajectory.joint_names = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7'] goal = FollowJointTrajectoryGoal() goal.trajectory = trajectory action_client.send_goal(goal) action_client.wait_for_result()
def move_joint(self, angles): goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = ['joint_{}'.format(i+1) for i in range(N_JOINTS)] point = JointTrajectoryPoint() point.positions = angles point.time_from_start = rospy.Duration(3) goal.trajectory.points.append(point) print self.jta.send_goal_and_wait(goal)
def go_to(self, collection_of_angles): joint_points = self._create_joint_trajectory_points( collection_of_angles) jt = JointTrajectory(joint_names=self.NAMES, points=joint_points) tolerance = self.duration + rospy.Duration(2) goal = FollowJointTrajectoryGoal(trajectory=jt, goal_time_tolerance=tolerance) self._send_goal_and_wait_for_result(goal)
def send_grasp_joint_positions(self): # values in range 0 ... 1 proximal_value = self._widget.proximal_spinbox.value() distal_value = self._widget.distal_spinbox.value() # define sets of joints proximal_joints = [ "sdh_thumb_2_joint", "sdh_finger_12_joint", "sdh_finger_22_joint" ] distal_joints = [ "sdh_thumb_3_joint", "sdh_finger_13_joint", "sdh_finger_23_joint" ] static_joints = ["sdh_knuckle_joint"] all_joints = static_joints + proximal_joints + distal_joints # map joint ranges from [0..1] to individual set ranges # proximal range: [-pi/2 .. 0] # distal range: [0 .. pi/2] proximal_range = [-math.pi / 2.0, 0.0] distal_range = [0.0, math.pi / 2.0] proximal_jpos = proximal_range[0] + proximal_value * ( proximal_range[1] - proximal_range[0]) distal_jpos = distal_range[0] + distal_value * (distal_range[1] - distal_range[0]) trajectory_goal = FollowJointTrajectoryGoal() # add single single joint point to trajectory trajectory_goal.trajectory.points.append(JointTrajectoryPoint()) for jname in all_joints: trajectory_goal.trajectory.joint_names.append(jname) # select joint position from set if jname in static_joints: trajectory_goal.trajectory.points[0].positions.append(0) elif jname in proximal_joints: trajectory_goal.trajectory.points[0].positions.append( proximal_jpos) elif jname in distal_joints: trajectory_goal.trajectory.points[0].positions.append( distal_jpos) else: raise Exception("joint not in set") # send trajectory and wait for response self.action_client.send_goal(trajectory_goal) if self.action_client.wait_for_result(timeout=rospy.Duration(5.0)): trajectory_result = self.action_client.get_result() print("set joints to " + ('%s' % trajectory_goal.trajectory.points[0].positions)) self.status_message.setText("set joints") return trajectory_result.error_code == FollowJointTrajectoryResult.SUCCESSFUL else: rospy.logerr("timeout while waiting for response from grasp goal") self.status_message.setText( "timeout while waiting for response from grasp goal") return False
class Elevation_up(py_trees.behaviour.Behaviour): #class Elevation_up(py_trees_ros.actions.ActionClient): def __init__(self, name="Elevation_up", target_pose=0.0): super(Elevation_up, self).__init__(name=name) self.target_pose = target_pose self.name = name self.client = actionlib.SimpleActionClient( '/arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.client.wait_for_server() # self.action_goal.trajectory.header.stamp = rospy.Time.now() # self.action_goal.goal_time_tolerance = rospy.Duration(30.0) def setup(self, timeout): return True def update(self): rospy.wait_for_service('/arm_controller/query_state') try: query_state = rospy.ServiceProxy('/arm_controller/query_state', QueryTrajectoryState) resp = query_state(rospy.Time.now()) except rospy.ServiceException, e: print "Service call failed: %s" % e joint_names = resp.name joint_positions = resp.position goal = FollowJointTrajectoryGoal() print(goal) goal.trajectory.joint_names = list(resp.name) point = JointTrajectoryPoint() point.positions = list(resp.position) # print(point) # print("point.positions[goal.trajectory.joint_names.index('elevation_joint')] : ", point.positions[goal.trajectory.joint_names.index('elevation_joint')] ) point.positions[goal.trajectory.joint_names.index( 'elevation_joint')] += self.target_pose if point.positions[goal.trajectory.joint_names.index( 'elevation_joint')] > 0: point.positions[goal.trajectory.joint_names.index( 'elevation_joint')] = 0 goal.trajectory.points.append(point) print(goal) point.time_from_start = rospy.Duration(1.0) self.client.send_goal(goal) self.client.wait_for_result() rospy.sleep(1.0) self.client.send_goal(goal) self.client.wait_for_result() print self.client.get_result() return py_trees.common.Status.SUCCESS
def _send_joint_trajectory(self, joints_references, timeout=rospy.Duration(5), joint_names=None): ''' Low level method that sends a array of joint references to the arm. If timeout is defined, it will wait for timeout*len(joints_reference) seconds for the completion of the actionlib goal. It will return True as soon as possible when the goal succeeded. On timeout, it will return False. ''' if not joints_references: return if not joint_names: if len(joints_references[0]) == len(self.joint_names) + len( self.torso_joint_names): joint_names = self.torso_joint_names + self.joint_names else: joint_names = self.joint_names time_from_start = rospy.Duration() ps = [] for joints_reference in joints_references: if len(joints_reference) != len(joint_names): rospy.logwarn( 'Please use the correct %d number of joint references (current = %d' % (len(joint_names), len(joints_references))) ps.append( JointTrajectoryPoint(positions=joints_reference, time_from_start=time_from_start)) joint_trajectory = JointTrajectory(joint_names=joint_names, points=ps) goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory, goal_time_tolerance=timeout) rospy.logdebug("Send {0} arm to jointcoords \n{1}".format( self.side, ps)) import time time.sleep( 0.001 ) # This is necessary: the rtt_actionlib in the hardware seems # to only have a queue size of 1 and runs at 1000 hz. This # means that if two goals are send approximately at the same # time (e.g. an arm goal and a torso goal), one of the two # goals probably won't make it. This sleep makes sure the # goals will always arrive in different update hooks in the # hardware TrajectoryActionLib server. self._ac_joint_traj.send_goal(goal) if timeout != rospy.Duration(0): done = self._ac_joint_traj.wait_for_result(timeout * len(joints_references)) if not done: rospy.logwarn("Cannot reach joint goal {0}".format(goal)) return done else: return None
def __init__(self, server_name): self.client = actionlib.SimpleActionClient( server_name, FollowJointTrajectoryAction) self.joint_positions = [] self.names = ["joint1", "joint2", "joint3", "joint4", "gripper"] # the list of xyz points we want to plan xyzrg_positions = [ # [25, -20, 5, 0, 0], # [25, -15, 5, 0, 0], # [25, -10, 5, 0, 0], # [25, -5, 5, 0, 0], # [25, 0, 5, 0, 0], # [25, 5, 5, 0, 0], # [25, 10, 5, 0, 0], # [25, 15, 5, 0, 0], # [25, 20, 5, 0, 0] # [20, -20, 5, 0, 0], # [20, 20, 5, 0, 0] # [30, 0, 30, 0, 0], # [30, 0, 25, 0, 0], # [30, 0, 20, 0, 0], # [30, 0, 15, 0, 0], # [30, 0, 10, 0, 0], # [30, 0, 5, 0, 0], # [30, 0, 0, 0, 0] # [30, 0, 30, 0, -pi/2], [30, 0, 15, 0, -pi / 2], [30, 0, 0, 0, -pi / 2], [30, 0, 0, 0, pi / 4], [30, 0, 15, 0, pi / 4], # [30, 0, 0, 0, pi/4], # [30, 0, 0, 0, -pi/2], [30, -13, 15, 0, pi / 4], [30, -13, 5, 0, pi / 4], [30, -13, 5, 0, -pi / 2], [30, -13, 15, 0, -pi / 2], # [0, 0, 55, 0, 0] ] # initial duration dur = rospy.Duration(2) # construct a list of joint positions by calling invkin for each xyz point for p in xyzrg_positions: jtp = JointTrajectoryPoint(positions=invkin(p), velocities=[0.5] * self.N_JOINTS, time_from_start=dur) dur += rospy.Duration(2) self.joint_positions.append(jtp) self.jt = JointTrajectory(joint_names=self.names, points=self.joint_positions) self.goal = FollowJointTrajectoryGoal(trajectory=self.jt, goal_time_tolerance=dur + rospy.Duration(2))
def send_trajectory(self,traj,stamp,acceleration=0.5,velocity=0.5,cartesian=False,linear=False): rate = rospy.Rate(30) t = rospy.Time(0) # Make sure that the trajectory 0 is the current joint position traj.points[0].positions = self.q0 if not self.valid_verify(stamp): return 'FAILURE - preempted' if self.simulation: if not linear: for pt in traj.points[:-1]: if not cartesian: self.send_q(pt.positions,acceleration,velocity) else: self.send_cart(pt.positions,acceleration,velocity) ## self.set_goal(pt.positions) start_t = rospy.Time.now() rospy.sleep(rospy.Duration(pt.time_from_start.to_sec() - t.to_sec())) t = pt.time_from_start if not cartesian: self.send_q(traj.points[-1].positions,acceleration,velocity) else: self.send_cart(traj.points[-1].positions,acceleration,velocity) ## self.set_goal(traj.points[-1].positions) start_t = rospy.Time.now() # wait until robot is at goal #while self.moving: while not self.at_goal: if (rospy.Time.now() - start_t).to_sec() > 3: return 'FAILURE - timeout' rate.sleep() if self.at_goal: return 'SUCCESS - moved to pose' else: return 'FAILURE - did not reach destination' else: self.set_goal(traj.points[-1].positions) goal = FollowJointTrajectoryGoal(trajectory=traj) if self.valid_verify(stamp): self.client.send_goal_and_wait(goal, preempt_timeout=rospy.Duration.from_sec(10.0)) # max time before returning = 30 s self.client.wait_for_result(rospy.Duration.from_sec(30.0)) res = self.client.get_result() if res is not None and res.error_code >= 0: return "SUCCESS" else: return "FAILURE - %s"%res.error_code
class Body_Rotate(py_trees.behaviour.Behaviour): #class Elevation_up(py_trees_ros.actions.ActionClient): def __init__(self, name="Body_rotation", x_offset=0.0, y_offset=0.0, z_offset=0.0): super(Body_Rotate, self).__init__(name=name) self.x_offset = x_offset self.y_offset = y_offset self.name = name self.client = actionlib.SimpleActionClient( '/arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.client.wait_for_server() def setup(self, timeout): return True def update(self): rospy.wait_for_service('/arm_controller/query_state') try: query_state = rospy.ServiceProxy('/arm_controller/query_state', QueryTrajectoryState) resp = query_state(rospy.Time.now()) except rospy.ServiceException, e: print "Service call failed: %s" % e joint_names = resp.name joint_positions = resp.position goal = FollowJointTrajectoryGoal() print(goal) goal.trajectory.joint_names = list(resp.name) point = JointTrajectoryPoint() point.positions = list(resp.position) theta = math.atan2(self.y_offset, self.x_offset) point.positions[goal.trajectory.joint_names.index( 'body_rotate_joint')] = theta # if point.positions[goal.trajectory.joint_names.index('elevation_joint')] > 0: # point.positions[goal.trajectory.joint_names.index('elevation_joint')] = 0 goal.trajectory.points.append(point) print(goal) point.time_from_start = rospy.Duration(1.0) self.client.send_goal(goal) self.client.wait_for_result() rospy.sleep(1.0) self.client.send_goal(goal) self.client.wait_for_result() print self.client.get_result() return py_trees.common.Status.SUCCESS
def __init__(self): rospy.init_node('trajectory_demo') # 是否需要回到初始位置 reset = rospy.get_param('~reset', False) arm_joints = [ 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6' ] # 调整目标位置 if reset: arm_goal = [0, 0, 0, 0, 0, 0] else: arm_goal = [-0.3, -1.0, 0.5, 0.8, 1.0, -0.7] # 连接机械臂轨迹规划的trajectory action server rospy.loginfo('waiting for arm trajectory controller...') arm_client = actionlib.SimpleActionClient( 'arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) arm_client.wait_for_server() rospy.loginfo('...connected.') arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].velocities = [0.0 for i in arm_joints] arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(3.0) rospy.loginfo('Moving the arm to goal position...') arm_goal = FollowJointTrajectoryGoal() arm_goal.trajectory = arm_trajectory arm_goal.goal_time_tolerance = rospy.Duration(0.0) arm_client.send_goal(arm_goal) arm_client.wait_for_result(rospy.Duration(5.0)) rospy.loginfo('...done')
def move1(): goal = FollowJointTrajectoryGoal() goal.trajectory = JointTrajectory() goal.trajectory.joint_names = JOINT_NAMES try: joint_states = rospy.wait_for_message("joint_states", JointState) joints_pos = joint_states.position goal.trajectory.points = [ JointTrajectoryPoint(positions=joints_pos, velocities=[0] * 6, time_from_start=rospy.Duration(0.0)), JointTrajectoryPoint(positions=Q1, velocities=[0] * 6, time_from_start=rospy.Duration(3.0)), ] client.send_goal(goal) client.wait_for_result() set_io(0, 1) # 吸气 time.sleep(1) joint_states = rospy.wait_for_message("joint_states", JointState) joints_pos = joint_states.position goal.trajectory.points = [ JointTrajectoryPoint(positions=joints_pos, velocities=[0] * 6, time_from_start=rospy.Duration(0.0)), JointTrajectoryPoint(positions=Q2, velocities=[0] * 6, time_from_start=rospy.Duration(3.0)), JointTrajectoryPoint(positions=Q3, velocities=[0] * 6, time_from_start=rospy.Duration(6.0)), ] client.send_goal(goal) client.wait_for_result() set_io(3, 1) # 吹气 time.sleep(1) set_io(0, 0) # 关闭io set_io(3, 0) except KeyboardInterrupt: client.cancel_goal() raise except: raise
def joints_move(self, angles, duration): goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = self.joint_names point = JointTrajectoryPoint() point.positions = angles point.time_from_start = rospy.Duration(duration) goal.trajectory.points.append(point) self.jta.send_goal_and_wait(goal) rospy.loginfo("Joint move {}".format(angles))
def move_joint(self, angles): # angles类型是元组,实参是所有关节转动的角度 goal = FollowJointTrajectoryGoal() #joint name should be the same as the tilt.yaml goal.trajectory.joint_names = ['Joint1', 'Joint2', 'Joint3', 'Joint4'] point = JointTrajectoryPoint() point.positions = angles # 得到每个关节点的位置 point.time_from_start = rospy.Duration(3) goal.trajectory.points.append(point) self.jta.send_goal_and_wait(goal)
def move_joint(self, angles): goal = FollowJointTrajectoryGoal() char = self.name[0] #either 'f' or 'b' goal.trajectory.joint_names = ['motor1','motor2','motor3','motor4','motor5','motor6','motor7','motor8','motor9','motor10','motor11','motor12','motor13','motor14','motor15','motor16','motor17','motor19'] point = JointTrajectoryPoint() point.positions = angles point.time_from_start = rospy.Duration(0.5) goal.trajectory.points.append(point) self.jta.send_goal_and_wait(goal)
def cb(self, move_base_goal): move_base_goal = self.tf.transformPose('map', move_base_goal.target_pose) goal_js = [ move_base_goal.pose.position.x, move_base_goal.pose.position.y, rotation_from_matrix( quaternion_matrix([ move_base_goal.pose.orientation.x, move_base_goal.pose.orientation.y, move_base_goal.pose.orientation.z, move_base_goal.pose.orientation.w ]))[0] ] current_js = rospy.wait_for_message('whole_body_controller/state', JointTrajectoryControllerState) goal = FollowJointTrajectoryGoal() goal.trajectory.header = move_base_goal.header goal.trajectory.joint_names = current_js.joint_names p = JointTrajectoryPoint() p.time_from_start = rospy.Duration(0.1) p.positions = current_js.actual.positions goal.trajectory.points.append(p) last_p = p def next_p(goal_p, last_p, f): diff = (goal_p - last_p) if diff > 0: diff = min(diff, self.max_vel * f) else: diff = max(diff, -self.max_vel * f) return last_p + (diff) while not np.allclose(np.array(goal_js), p.positions[:3]): p = JointTrajectoryPoint() p.time_from_start = last_p.time_from_start + rospy.Duration( 1 * self.freq) p.positions = list(current_js.actual.positions) p.positions[0] = next_p(goal_js[0], last_p.positions[0], self.freq) p.positions[1] = next_p(goal_js[1], last_p.positions[1], self.freq) p.positions[2] = next_p(goal_js[2], last_p.positions[2], self.freq) goal.trajectory.points.append(p) last_p = p self.client.send_goal(goal) while not self.client.wait_for_result(rospy.Duration(.1)): if self.server.is_preempt_requested(): rospy.loginfo('new goal, cancel old one') self.client.cancel_all_goals() self.server.set_preempted(MoveBaseResult()) break # result = self.client.get_result() # if result.error_code == result.SUCCESSFUL: self.server.set_succeeded(MoveBaseResult())
def __init__(self, arm_name): """ Initializing the Parameters and the Client definition to send to the controller """ self.goal = FollowJointTrajectoryGoal() self.client = actionlib.SimpleActionClient( arm_name + '/joint_trajectory_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction) self.client.wait_for_server(rospy.Duration(5))
def move_joint(self, angles): goal = FollowJointTrajectoryGoal() char = self.name[0] goal.trajectory.joint_names = ['head_pan_joint', 'head_tilt_joint'] point = JointTrajectoryPoint() point.positions = angles point.time_from_start = rospy.Duration(1) goal.trajectory.points.append(point) self.jta.send_goal_and_wait(goal)
def move_joint(self, angles): goal = FollowJointTrajectoryGoal() #joint name should be the same as the tilt.yaml goal.trajectory.joint_names = ['Joint11', 'Joint12','Joint13','Joint14','Joint21', 'Joint22','Joint23','Joint24'] point = JointTrajectoryPoint() point.positions = angles point.time_from_start = rospy.Duration(0.5) goal.trajectory.points.append(point) self.jta.send_goal_and_wait(goal)
def move_joint(self, angles): goal = FollowJointTrajectoryGoal() char = self.name[0] #either 'f' or 'b' goal.trajectory.joint_names = ['joint_1'+char, 'joint_2'+char,'joint_3'+char] point = JointTrajectoryPoint() point.positions = angles point.time_from_start = rospy.Duration(3) goal.trajectory.points.append(point) self.jta.send_goal_and_wait(goal)
def test_fjta_overwrite_goal(self): # for >= kinetic if os.environ['ROS_DISTRO'] < 'kinetic': return True from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal larm = actionlib.SimpleActionClient( "/larm_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction) self.impl_test_jta_overwrite_goal(larm, FollowJointTrajectoryGoal())
def receiverLoop(self): while True: try: input_msg = pickle.load( open( "/dev/shm/right_arm_goal.p", "rb" ) ) os.remove("/dev/shm/right_arm_goal.p") except IOError: print "file not there..." rospy.sleep(0.1) continue print input_msg goal = FollowJointTrajectoryGoal() rospy.loginfo("input_msg.goal.goal_time_tolerance: \n" + str(input_msg.goal.goal_time_tolerance ) ) goal.goal_time_tolerance = rospy.Duration(secs=0, nsecs=0) rospy.loginfo("input_msg.goal.goal_tolerance: \n" + str(input_msg.goal.goal_tolerance ) ) goal.goal_tolerance = input_msg.goal.goal_tolerance rospy.loginfo("input_msg.goal.path_tolerance: \n" + str(input_msg.goal.path_tolerance ) ) goal.path_tolerance = input_msg.goal.path_tolerance rospy.loginfo("input_msg.goal.trajectory.header: \n" + str(input_msg.goal.trajectory.header ) ) goal.trajectory.header = input_msg.goal.trajectory.header goal.trajectory.header.frame_id = '/base_link' goal.trajectory.joint_names = input_msg.goal.trajectory.joint_names input_times = pickle.load( open( "/dev/shm/right_arm_goal_times.p", "rb" ) ) rospy.loginfo("input_times is:\n" + str(input_times)) for input_point, time in zip(input_msg.goal.trajectory.points, input_times): p = JointTrajectoryPoint() p.accelerations = input_point.accelerations p.positions = input_point.positions p.velocities = input_point.velocities # rospy.loginfo("???????????????????????????????????????????????????????????????") # rospy.loginfo("Time from start is:\n" + str(input_point.time_from_start)) # rospy.loginfo("time is:\n" + str(time)) # rospy.loginfo("???????????????????????????????????????????????????????????????") p.time_from_start = time goal.trajectory.points.append(p) os.remove("/dev/shm/right_arm_goal_times.p") #print "Goal will be:" rospy.loginfo("!!!!!!!!!!!Goal:\n" +str(goal)) self.right_arm_as.send_goal(goal) rospy.loginfo("Waiting for result") self.right_arm_as.wait_for_result() rospy.loginfo("Goal done")
def move_to(self, positions, velocities, accelerations, delta_t): assert(len(self.joint_names) == positions.shape[1]) trajectory = JointTrajectory() trajectory.joint_names = self.joint_names for i in range(positions.shape[0]): # for each row i p = JointTrajectoryPoint() p.positions = list(positions[i]) p.velocities = list(velocities[i]) p.accelerations = list(accelerations[i]) p.time_from_start = rospy.Duration(i * delta_t) trajectory.points.append(p) follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = trajectory self.client.send_goal(follow_goal) self.client.wait_for_result()
def enviarTrayectoria(self, arm_goal1): # arm_goal = [0, 0, 0, 0, 0, 0] # arm_goal = [-0.3, 0.5, -1.0, 1.8, 0.3, 0.6] arm_joints = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') rospy.loginfo('...connected.') # Create a single-point arm trajectory with the arm_goal as the end-point arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal1 arm_trajectory.points[0].velocities = [0.0 for i in arm_joints] arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(1.5) rospy.loginfo('Moving the arm to goal position...') # Create an empty trajectory goal arm_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above arm_goal.trajectory = arm_trajectory # Specify zero tolerance for the execution time arm_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal to the action server self.arm_client.send_goal(arm_goal) # Wait for up to 5 seconds for the motion to complete self.arm_client.wait_for_result(rospy.Duration(5.0)) self.actualizar_IK(arm_goal1) rospy.loginfo('...done')
def execute(self, traj_msg): """ Execute a JointTrajectory message and return a TrajectoryFuture. @param traj_msg: requested trajectory @type traj_msg: trajectory_msgs.msg.JointTrajectory @return future representing the execution of the trajectory @rtype TrajectoryFuture """ from control_msgs.msg import FollowJointTrajectoryGoal goal_msg = FollowJointTrajectoryGoal() goal_msg.trajectory = traj_msg traj_future = TrajectoryFuture(traj_msg) traj_future._handle = self._client.send_goal( goal_msg, transition_cb=traj_future.on_transition, feedback_cb=traj_future.on_feedback ) return traj_future
def tuck_arm(self): while not self.state_recv: rospy.loginfo("Waiting for controllers to be up...") rospy.sleep(0.1) trajectory = JointTrajectory() trajectory.joint_names = self.joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = self.tucked trajectory.points[0].velocities = [0.0 for i in self.joint_names] trajectory.points[0].accelerations = [0.0 for i in self.joint_names] trajectory.points[0].time_from_start = rospy.Duration(5.0) rospy.loginfo("Tucking arm...") goal = FollowJointTrajectoryGoal() goal.trajectory = trajectory goal.goal_time_tolerance = rospy.Duration(0.0) self.client.send_goal(goal) self.client.wait_for_result(rospy.Duration(6.0)) rospy.loginfo("...done")
def execute_cb(self, goal): """ :type goal: FollowJointTrajectoryGoal """ rospy.loginfo("Got a goal!") # Just resend the goal to the real as # while checking for cancel goals # Create goal g = FollowJointTrajectoryGoal() # We ignore path_tolerance and goal_tolerance... does not make sense in # a gripper g.goal_time_tolerance = goal.goal_time_tolerance jt = self.substitute_trajectory(goal.trajectory) g.trajectory = jt if not self._ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr("Action server for gripper not found!") self._as.set_aborted(FollowJointTrajectoryResult()) # Send goal subscribing to feedback to republish it self._ac.send_goal(g, feedback_cb=self._feedback_cb) # wait for goal to finish while checking if cancel is requested while not self._ac.wait_for_result(rospy.Duration(0.1)): rospy.loginfo("Waiting for goal to finish...") # Deal with goal cancelled if self._as.is_preempt_requested(): self._ac.cancel_all_goals() self._as.set_preempted() return result = self._ac.get_result() res = FollowJointTrajectoryResult() if result: res.error_code = FollowJointTrajectoryResult.SUCCESSFUL self._as.set_succeeded(res) else: res.error_code = result self._as.set_aborted(res)
def update(self): points = [] tick = 0 self.phase = np.random.uniform(np.pi) while tick <= self.time: x = 2*np.pi*self.freq*tick + self.phase positions = [np.sin(x)]*len(self.joint_names) velocities = [np.cos(x)]*len(self.joint_names) accelerations = [-np.sin(x)]*len(self.joint_names) points.append(JointTrajectoryPoint(positions=positions, velocities=velocities, accelerations=accelerations, time_from_start=rospy.Time.from_sec(tick))) tick += self.step msg = JointTrajectory(joint_names=self.joint_names, points=points) msg.header.stamp = rospy.Time.now() self.pub.publish(msg) # joint_trajectroy_action goal = FollowJointTrajectoryGoal() goal.trajectory = msg self.client.send_goal(goal)
def move_to_joint(end_pos, duration): print end_pos traj = build_traj(get_cur_pos(), end_pos, duration) # wait for subscribers to connect # pub = rospy.Publisher('joint_path_command', JointTrajectory) # if not wait_for_subs(pub, 1, 0.5, 2.0): # rospy.logwarn('Timeout while waiting for subscribers. Publishing trajectory anyway.') # pub.publish(traj) _left_client = actionlib.SimpleActionClient( '/joint_trajectory_action', FollowJointTrajectoryAction, ) print "Waiting..." _left_client.wait_for_server(); print "Done waiting" goal = FollowJointTrajectoryGoal() goal.trajectory = traj goal.trajectory.header.stamp = rospy.Time.now() _left_client.send_goal(goal) print "Sent goal" rate = rospy.Rate(60) # hz counter = 0 while counter < duration*70: rospy.is_shutdown() rate.sleep() counter = counter+1 print "Received Result: "