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 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 __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 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 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 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 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 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 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 __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 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 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_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 __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 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: "
def __init__(self): rospy.init_node('trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set to False to wait for arm to finish before moving head sync = rospy.get_param('~sync', True) # Which joints define the arm? arm_joints = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ] # Which joints define the head? head_joints = ['head_pan_joint', 'head_tilt_joint'] # Which joint defines the torso? torso_joints = ['torso_lift_joint'] if reset: # Set the arm back to the tucked position arm_goal = [-1.3901, 1.3439, -2.8327, -1.8119, 0.0, -1.6571, 0.0] # Re-center the head head_goal = [0.0, 0.0] # Bring the toros back down torso_goal = [0.0] else: # Set a goal configuration for the arm arm_goal = [-1.0, 0, 0, -1.0, 0, 0, 0] # Set a goal configuration for the head head_goal = [-0.85, -0.25] # Move the torso up torso_goal = [0.35] # Connect to the arm 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.') # 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.') # Connect to the torso trajectory action server rospy.loginfo('Waiting for headtorso trajectory controller...') torso_client = actionlib.SimpleActionClient( 'torso_controller/follow_joint_trajectory', FollowJointTrajectoryAction) torso_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(5.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) if not sync: # Wait for up to 5 seconds for the motion to complete arm_client.wait_for_result(rospy.Duration(5.0)) # 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 = head_goal 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(5.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 head_client.send_goal(head_goal) if not sync: # Wait for up to 5 seconds for the motion to complete head_client.wait_for_result(rospy.Duration(5.0)) # Create a single-point torso trajectory with the torso_goal as the end-point torso_trajectory = JointTrajectory() torso_trajectory.joint_names = torso_joints torso_trajectory.points.append(JointTrajectoryPoint()) torso_trajectory.points[0].positions = torso_goal torso_trajectory.points[0].velocities = [0.0 for i in torso_joints] torso_trajectory.points[0].accelerations = [0.0 for i in torso_joints] torso_trajectory.points[0].time_from_start = rospy.Duration(5.0) # Send the trajectory to the head action server rospy.loginfo('Moving the head to goal position...') torso_goal = FollowJointTrajectoryGoal() torso_goal.trajectory = torso_trajectory torso_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal torso_client.send_goal(torso_goal) # Wait for up to 8 seconds for the motion to complete torso_client.wait_for_result(rospy.Duration(8.0)) rospy.loginfo('...done')
torso_client = actionlib.SimpleActionClient( "torso_controller/follow_joint_trajectory", FollowJointTrajectoryAction) torso_client.wait_for_server() rospy.loginfo("...connected.") trajectory = JointTrajectory() trajectory.points.append(JointTrajectoryPoint()) trajectory.joint_names = torso_joint_names trajectory.points[0].positions = torso_joint_positions trajectory.points[0].velocities = [0.0] * len(torso_joint_positions) trajectory.points[0].accelerations = [0.0] * len(torso_joint_positions) trajectory.points[0].time_from_start = rospy.Duration(5.0) torso_goal = FollowJointTrajectoryGoal() torso_goal.trajectory = trajectory torso_goal.goal_time_tolerance = rospy.Duration(0.0) torso_client.send_goal(torso_goal) torso_client.wait_for_result(rospy.Duration(5.0)) """ rospy.loginfo("Waiting for head_controller...") head_client = actionlib.SimpleActionClient("head_controller/follow_joint_trajectory", FollowJointTrajectoryAction) head_client.wait_for_server() rospy.loginfo("...connected.") rospy.loginfo("Waiting for arm_controller...") arm_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) arm_client.wait_for_server() rospy.loginfo("...connected.")
def execute_plan_traj(self, plannedTraj): 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()
def follow_trajectory(self, trajectory): follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = trajectory self.client.send_goal(follow_goal) self.client.wait_for_result()
def get_default_pose(): arm_joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] # arm_intermediate_positions = [1.32, 0, -1.4, 1.72, 0.0, 1.66, 0.0] arm_intermediate_positions = [0.0, -0.62, 0, 0, 0.0, 0.62, 0.0] # arm_joint_positions = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] # arm_joint_positions = [1.1, -0.64, -1.83, 0.96, 1.13, -.96, 0.0] arm_joint_positions = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] head_joint_names = ["head_pan_joint", "head_tilt_joint"] head_joint_positions = [0.0, 0.0] #rospy.init_node("prepare_simulated_robot_pick_place") # Check robot serial number, we never want to run this on a real robot! if rospy.get_param("robot/serial") != "ABCDEFGHIJKLMNOPQRSTUVWX": rospy.logerr("This script should not be run on a real robot") sys.exit(-1) # rospy.loginfo("Waiting for move_base...") # move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) # move_base.wait_for_server() # rospy.loginfo("...connected.") rospy.loginfo("Waiting for head_controller...") head_client = actionlib.SimpleActionClient( "head_controller/follow_joint_trajectory", FollowJointTrajectoryAction) head_client.wait_for_server() rospy.loginfo("...connected.") rospy.loginfo("Waiting for arm_controller...") arm_client = actionlib.SimpleActionClient( "arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) arm_client.wait_for_server() rospy.loginfo("...connected.") rospy.loginfo("Waiting for gripper_controller...") gripper_client = actionlib.SimpleActionClient( "gripper_controller/gripper_action", GripperCommandAction) gripper_client.wait_for_server() rospy.loginfo("...connected.") # move_goal = MoveBaseGoal() # move_goal.target_pose.pose.position.x = x # move_goal.target_pose.pose.position.y = y # move_goal.target_pose.pose.orientation.z = sin(theta/2.0) # move_goal.target_pose.pose.orientation.w = cos(theta/2.0) # move_goal.target_pose.header.frame_id = frame # move_goal.target_pose.header.stamp = rospy.Time.now() # move_base.send_goal(move_goal) # move_base.wait_for_result() 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(5.0) head_goal = FollowJointTrajectoryGoal() head_goal.trajectory = trajectory head_goal.goal_time_tolerance = rospy.Duration(0.0) trajectory = JointTrajectory() trajectory.joint_names = arm_joint_names # trajectory.points.append(JointTrajectoryPoint()) # trajectory.points[0].positions = [0.0] * len(arm_joint_positions) # trajectory.points[0].velocities = [0.0] * len(arm_joint_positions) # trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions) # duration = 5.0 # trajectory.points[0].time_from_start = rospy.Duration(duration) # # trajectory.points.append(JointTrajectoryPoint()) # trajectory.points[1].positions = arm_intermediate_positions # trajectory.points[1].velocities = [0.0] * len(arm_joint_positions) # trajectory.points[1].accelerations = [0.0] * len(arm_joint_positions) # duration += 5.0 # trajectory.points[1].time_from_start = rospy.Duration(duration) # # trajectory.points.append(JointTrajectoryPoint()) # trajectory.points[2].positions = arm_joint_positions # trajectory.points[2].velocities = [0.0] * len(arm_joint_positions) # trajectory.points[2].accelerations = [0.0] * len(arm_joint_positions) # duration += 5.0 # trajectory.points[2].time_from_start = rospy.Duration(duration) i = 0 duration = 5.0 trajectory.points.append(JointTrajectoryPoint()) arm_joint_positions = [0.0, -1.22, 0.0, 1.20, 0.0, 1.6, 0.0] # above the block, READY TO PICK trajectory.points[i].positions = arm_joint_positions trajectory.points[i].velocities = [0.0] * len(arm_joint_positions) trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions) trajectory.points[i].time_from_start = rospy.Duration(duration) i += 1 duration += 5.0 trajectory.points.append(JointTrajectoryPoint()) arm_joint_positions = [0.0, -1.20, 0.0, 1.45, 0.0, 1.3, 0.0] # PERFECT - PICK POSE trajectory.points[i].positions = arm_joint_positions trajectory.points[i].velocities = [0.0] * len(arm_joint_positions) trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions) trajectory.points[i].time_from_start = rospy.Duration(duration) # i += 1 # duration += 5.0 # trajectory.points.append(JointTrajectoryPoint()) # arm_joint_positions = [0.0, -1.22, 0.0, 1.20, 0.0, 1.6, 0.0] # above the block, READY TO PICK # trajectory.points[i].positions = arm_joint_positions # trajectory.points[i].velocities = [0.0] * len(arm_joint_positions) # trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions) # trajectory.points[i].time_from_start = rospy.Duration(duration) arm_goal = FollowJointTrajectoryGoal() arm_goal.trajectory = trajectory arm_goal.goal_time_tolerance = rospy.Duration(0.0) gripper_goal = GripperCommandGoal() gripper_goal.command.max_effort = 10.0 gripper_goal.command.position = 0.1 rospy.loginfo("Setting positions...") head_client.send_goal(head_goal) arm_client.send_goal(arm_goal) gripper_client.send_goal(gripper_goal) gripper_client.wait_for_result(rospy.Duration(5.0)) arm_client.wait_for_result(rospy.Duration(6.0)) head_client.wait_for_result(rospy.Duration(6.0)) rospy.loginfo("...done") gripper_goal = GripperCommandGoal() gripper_goal.command.max_effort = 10.0 gripper_goal.command.position = 0.0 #gripper_client.send_goal(gripper_goal) time.sleep(5.0) gripper_client.send_goal_and_wait(gripper_goal, rospy.Duration(5.0)) #gripper_client.wait_for_result(rospy.Duration(100.0)) rospy.loginfo("...done") trajectory = JointTrajectory() trajectory.joint_names = arm_joint_names i = 0 duration = 5.0 trajectory.points.append(JointTrajectoryPoint()) arm_joint_positions = [0.0, -1.22, 0.0, 1.20, 0.0, 1.6, 0.0] # above the block, READY TO PICK trajectory.points[i].positions = arm_joint_positions trajectory.points[i].velocities = [0.0] * len(arm_joint_positions) trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions) trajectory.points[i].time_from_start = rospy.Duration(duration) arm_goal = FollowJointTrajectoryGoal() arm_goal.trajectory = trajectory arm_goal.goal_time_tolerance = rospy.Duration(0.0) arm_client.send_goal(arm_goal) arm_client.wait_for_result(rospy.Duration(6.0))
def __init__(self): rospy.init_node('trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set to False to wait for arm to finish before moving head sync = rospy.get_param('~sync', True) # Which joints define the arm? arm_joints = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] # Which joints define the head? #head_joints = ['head_pan_joint', 'head_tilt_joint'] if reset: # Set the arm back to the resting position arm_goal = [0, 0, 0, 0, 0, 0] # Re-center the head head_goal = [0, 0] else: # Set a goal configuration for the arm arm_goal = [-0.3, -2.0, -1.0, 0.8, 1.0, -0.7] # Set a goal configuration for the head head_goal = [-1.3, -0.1] # 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.') # 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 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) if not sync: # Wait for up to 5 seconds for the motion to complete arm_client.wait_for_result(rospy.Duration(5.0)) # 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 = head_goal 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 head_client.send_goal(head_goal) # Wait for up to 5 seconds for the motion to complete head_client.wait_for_result(rospy.Duration(5.0)) rospy.loginfo('...done')
rospy.loginfo("Waiting for gripper_controller...") gripper_client = actionlib.SimpleActionClient( "gripper_controller/gripper_action", GripperCommandAction) gripper_client.wait_for_server() rospy.loginfo("...connected.") 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(5.0) head_goal = FollowJointTrajectoryGoal() head_goal.trajectory = trajectory head_goal.goal_time_tolerance = rospy.Duration(0.0) trajectory = JointTrajectory() trajectory.joint_names = arm_joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = [0.0] * len(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(1.0) trajectory.points.append(JointTrajectoryPoint()) trajectory.points[1].positions = arm_intermediate_positions trajectory.points[1].velocities = [0.0] * len(arm_joint_positions) trajectory.points[1].accelerations = [0.0] * len(arm_joint_positions) trajectory.points[1].time_from_start = rospy.Duration(4.0) trajectory.points.append(JointTrajectoryPoint())
trajectory.points[ 0].positions = arm_joint_positions5 #Asignamos la trayectoria a ejecutar trajectory.points[0].velocities = [0.0] * len( arm_joint_positions0) #Asignamos los valores por default = 0 trajectory.points[0].accelerations = [0.0] * len( arm_joint_positions0) #Asignamos los valores por default = 0 trajectory.points[0].time_from_start = rospy.Duration( 1.0) #Asignamos la duración de 1seg para iniciar arm_goal = FollowJointTrajectoryGoal() #Creamos una variable de tipo Goal arm_goal.trajectory = trajectory #Asignamos nuestra trayectoria hacia la trayectoria de nuestro objetivo(goal) arm_goal.goal_time_tolerance = rospy.Duration( 0.0 ) #Asignamos la duración de 0seg como tiempo de tolerancia de nuestro objetivo rospy.loginfo("Setting positions...") #Imprimimos en pantalla arm_client.send_goal( arm_goal ) #Enviamos nuestro objetivo hacia el Acción Server de nuestro robot Fetch arm_client.wait_for_result( rospy.Duration(6.0)) #Esperamos 6 segundos para obtener los resultados rospy.loginfo(
arm_client.wait_for_server() rospy.loginfo("...connected.") trajectory = JointTrajectory() trajectory.joint_names = arm_joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = [0.0] * len(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(1.0) trajectory.points.append(JointTrajectoryPoint()) trajectory.points[1].positions = arm_intermediate_positions trajectory.points[1].velocities = [0.0] * len(arm_joint_positions) trajectory.points[1].accelerations = [0.0] * len(arm_joint_positions) trajectory.points[1].time_from_start = rospy.Duration(4.0) trajectory.points.append(JointTrajectoryPoint()) trajectory.points[2].positions = arm_joint_positions trajectory.points[2].velocities = [0.0] * len(arm_joint_positions) trajectory.points[2].accelerations = [0.0] * len(arm_joint_positions) trajectory.points[2].time_from_start = rospy.Duration(7.5) arm_goal = FollowJointTrajectoryGoal() arm_goal.trajectory = trajectory arm_goal.goal_time_tolerance = rospy.Duration(0.0) rospy.loginfo("Setting positions...") arm_client.send_goal(arm_goal) arm_client.wait_for_result(rospy.Duration(6.0)) rospy.loginfo("...done")
def execute(self, component_name, as_name, target="", blocking=True): ah = ActionHandle("move_joint_trajectory", component_name, target, blocking) rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target) # get joint_names from parameter server param_string = self.ns_global_prefix + "/" + component_name + "/joint_names" if not rospy.has_param(param_string): rospy.logerr( "parameter %s does not exist on ROS Parameter Server, aborting...", param_string) ah.set_failed(2) return ah joint_names = rospy.get_param(param_string) # check joint_names parameter if not type(joint_names) is list: # check list rospy.logerr( "no valid joint_names for %s: not a list, aborting...", component_name) print "joint_names are:", joint_names ah.set_failed(3) return ah else: for i in joint_names: #print i,"type1 = ", type(i) if not type(i) is str: # check string rospy.logerr( "no valid joint_names for %s: not a list of strings, aborting...", component_name) print "joint_names are:", param ah.set_failed(3) return ah else: rospy.logdebug("accepted joint_names for component %s", component_name) # get joint values from parameter server if type(target) is str: if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target): rospy.logerr( "parameter %s does not exist on ROS Parameter Server, aborting...", self.ns_global_prefix + "/" + component_name + "/" + target) ah.set_failed(2) return ah param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target) else: param = target # check trajectory parameters if not type(param) is list: # check outer list rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name) print "parameter is:", param ah.set_failed(3) return ah traj = [] for point in param: #print point,"type1 = ", type(point) if type(point) is str: if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + point): rospy.logerr( "parameter %s does not exist on ROS Parameter Server, aborting...", self.ns_global_prefix + "/" + component_name + "/" + point) ah.set_failed(2) return ah point = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + point) point = point[ 0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories #print point elif type(point) is list: rospy.logdebug("point is a list") else: rospy.logerr( "no valid parameter for %s: not a list of lists or strings, aborting...", component_name) print "parameter is:", param ah.set_failed(3) return ah # here: point should be list of floats/ints #print point if not len(point) == len(joint_names): # check dimension rospy.logerr( "no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, len(joint_names), len(point)) print "parameter is:", param ah.set_failed(3) return ah for value in point: #print value,"type2 = ", type(value) if not ((type(value) is float) or (type(value) is int)): # check type #print type(value) rospy.logerr( "no valid parameter for %s: not a list of float or int, aborting...", component_name) print "parameter is:", param ah.set_failed(3) return ah rospy.logdebug("accepted value %f for %s", value, component_name) traj.append(point) rospy.logdebug("accepted trajectory for %s", component_name) # convert to ROS trajectory message traj_msg = JointTrajectory() traj_msg.header.stamp = rospy.Time.now() + rospy.Duration(0.5) traj_msg.joint_names = joint_names point_nr = 0 for point in traj: point_nr = point_nr + 1 point_msg = JointTrajectoryPoint() point_msg.positions = point point_msg.velocities = [0] * len(joint_names) point_msg.time_from_start = rospy.Duration( 3 * point_nr ) # this value is set to 3 sec per point. \todo TODO: read from parameter traj_msg.points.append(point_msg) # call action server action_server_name = "/" + component_name + '_controller/follow_joint_trajectory' rospy.logdebug("calling %s action server", action_server_name) client = actionlib.SimpleActionClient(action_server_name, FollowJointTrajectoryAction) # trying to connect to server rospy.logdebug("waiting for %s action server to start", action_server_name) if not client.wait_for_server(rospy.Duration(5)): # error: server did not respond rospy.logerr( "%s action server not ready within timeout, aborting...", action_server_name) ah.set_failed(4) return ah else: rospy.logdebug("%s action server ready", action_server_name) # sending goal client_goal = FollowJointTrajectoryGoal() client_goal.trajectory = traj_msg #print client_goal client.send_goal(client_goal) ah.set_client(client) ah.wait_inside() return ah
def movel(self, destino, orientation=None, speed=None): """ Move to destination following a line args: destino: a list of xyz defining a point in space orientation: a quaternion defining the orientation """ self.client.cancel_all_goals() # to create uniform movements lets define speed to calculate time if speed is None: speed = 0.4 # m/s cur_pose = self.fwd_kin(self.joints_state, i_unit='r', o_unit='p') if orientation is None: orientation = cur_pose.orientation if type(destino) == type(Pose()): desired_position = np.array([ destino.position.x, destino.position.y, destino.position.z, destino.orientation.x, destino.orientation.y, destino.orientation.z, destino.orientation.w ]) tmp1 = np.array([ cur_pose.position.x, cur_pose.position.y, cur_pose.position.z ]) tmp2 = np.array( [destino.position.x, destino.position.y, destino.position.z]) else: desired_position = np.array([ *destino, orientation.x, orientation.y, orientation.z, orientation.w ]) tmp1 = np.array([ cur_pose.position.x, cur_pose.position.y, cur_pose.position.z ]) tmp2 = np.array(destino) distance = np.linalg.norm(tmp1 - tmp2) if distance <= 0: nanoseconds = 1_000_000 else: nanoseconds = 1_000_000_000 * distance / speed # check if it is possible to reach if len(self.check_working_space([tmp2.tolist()])) == 0: rospy.logerr('Position %s out of reach' % np.array_str(tmp2, precision=2, suppress_small=True)) return False # cur_pose = np.array([cur_pose.position.x,cur_pose.position.y,cur_pose.position.z]) lista = self.trajectory( desired_position, cur_pose ) # returns a list of np array with position and orientation pose = Pose() g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = self.JOINT_NAMES joints_inv_kine = self.joints_state g.trajectory.points = [] n = len(lista) if self.debug: os.chdir('/home/ubuntu/Documents/debug') new_lista = [] for item in lista: new_lista.append( np.array_str(item, precision=2, suppress_small=True)) with open(time.strftime("%Y%m%d-%H%M%S") + ' lista.yaml', 'w') as file: yaml.dump(new_lista, file) for i, point in enumerate(lista): pose.position.x = point[0] pose.position.y = point[1] pose.position.z = point[2] pose.orientation.x = point[3] pose.orientation.y = point[4] pose.orientation.z = point[5] pose.orientation.w = point[6] a = self.inv_kine(self.ros2np(pose)) a = a.transpose().tolist() joints_inv_kine = self.select(a, joints_inv_kine) t = int(i * nanoseconds / n) g.trajectory.points.append( JointTrajectoryPoint(positions=joints_inv_kine, velocities=[0] * 6, time_from_start=rospy.Duration(0, t))) # Callback goal self.done = False self.client.send_goal(g, done_cb=self.done_callback) start_time = time.time() while not self.done and time.time( ) - start_time < 3: # 3 seconds timeout time.sleep(0.05) if not self.done: rospy.logerr('Canceling movement') self.client.cancel_all_goals() return self.done
def joint_traj_jtas(self, j_traj): self.client.wait_for_server() goal = FollowJointTrajectoryGoal() goal.trajectory = j_traj self.client.send_goal(goal)
def __init__(self): x = loadmat('cuadrupedo_andar.mat') # print x piernas_der = x['estruc_patas_rec'] piernaLF = piernas_der[0, 0] piernaLR = piernas_der[0, 1] piernaRF = piernas_der[0, 2] piernaRR = piernas_der[0, 3] rospy.init_node('trajectory_cua') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set to False to wait for arm to finish before moving head sync = rospy.get_param('~sync', True) # Which joints define the arm? piernas_joints_RR = [ 'coxa_joint_RR', 'femur_joint_RR', 'tibia_joint_RR', 'tarsus_joint_RR' ] piernas_joints_RF = [ 'coxa_joint_RF', 'femur_joint_RF', 'tibia_joint_RF', 'tarsus_joint_RF' ] piernas_joints_LR = [ 'coxa_joint_LR', 'femur_joint_LR', 'tibia_joint_LR', 'tarsus_joint_LR' ] piernas_joints_LF = [ 'coxa_joint_LF', 'femur_joint_LF', 'tibia_joint_LF', 'tarsus_joint_LF' ] rospy.loginfo('Waiting for right arm trajectory controller...') rr_client = actionlib.SimpleActionClient( '/cuadrupedo/pata_rr/follow_joint_trajectory', FollowJointTrajectoryAction) rr_client.wait_for_server() rf_client = actionlib.SimpleActionClient( '/cuadrupedo/pata_rf/follow_joint_trajectory', FollowJointTrajectoryAction) rf_client.wait_for_server() lr_client = actionlib.SimpleActionClient( '/cuadrupedo/pata_lr/follow_joint_trajectory', FollowJointTrajectoryAction) lr_client.wait_for_server() lf_client = actionlib.SimpleActionClient( '/cuadrupedo/pata_lf/follow_joint_trajectory', FollowJointTrajectoryAction) lf_client.wait_for_server() rospy.loginfo('...connected.') rr_trajectory1 = JointTrajectory() rr_trajectory1.joint_names = piernas_joints_RR rf_trajectory1 = JointTrajectory() rf_trajectory1.joint_names = piernas_joints_RF lr_trajectory1 = JointTrajectory() lr_trajectory1.joint_names = piernas_joints_LR lf_trajectory1 = JointTrajectory() lf_trajectory1.joint_names = piernas_joints_LF # piernas_goalF0 = [0, -0.3, -0.4, 0.8, -0.3, -0.4, # 0, -0.4, -0.3, 0.6, +0.4, -0.3] # arm_trajectory.points.append(JointTrajectoryPoint()) # arm_trajectory.points[0].positions = piernas_goalF0 # arm_trajectory.points[0].velocities = [0.0 for k in piernas_goalF0] # arm_trajectory.points[0].accelerations = [0.0 for k in piernas_goalF0] # arm_trajectory.points[0].time_from_start = rospy.Duration(2) piernasLF_goalF01 = [ 0, piernaLF['nuevo_femur'][0, 0], -piernaLF['nuevo_tibia'][0, 0], -piernaLF['nuevo_tarsus'][0, 0] ] lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[0].positions = piernasLF_goalF01 lf_trajectory1.points[0].time_from_start = rospy.Duration(1) piernasLR_goalF01 = [ 0, piernaLR['nuevo_femur'][0, 0], -piernaLR['nuevo_tibia'][0, 0], -piernaLR['nuevo_tarsus'][0, 0] ] lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[0].positions = piernasLR_goalF01 lr_trajectory1.points[0].time_from_start = rospy.Duration(1) piernasRF_goalF01 = [ 0, -piernaRF['nuevo_femur'][0, 0], piernaRF['nuevo_tibia'][0, 0], piernaRF['nuevo_tarsus'][0, 0] ] rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[0].positions = piernasRF_goalF01 rf_trajectory1.points[0].time_from_start = rospy.Duration(1) piernasRR_goalF01 = [ 0, -piernaRR['nuevo_femur'][0, 0], piernaRR['nuevo_tibia'][0, 0], piernaRR['nuevo_tarsus'][0, 0] ] rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[0].positions = piernasRR_goalF01 rr_trajectory1.points[0].time_from_start = rospy.Duration(1) ind = 1 salto = 5 for i in range(1, int(piernaRR['nuevo_tarsus'].shape[0] * 0.3), salto): piernasLF_goalF01 = [ 0, piernaLF['nuevo_femur'][i, 0], piernaLF['nuevo_tibia'][i, 0], piernaLF['nuevo_tarsus'][i, 0] ] lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[ind].positions = piernasLF_goalF01 # lf_trajectory1.points[ind].velocities = [0.0 for k in piernasLF_goalF01] # lf_trajectory1.points[ind].accelerations = [0.0 for k in piernasLF_goalF01] piernasLR_goalF01 = [ 0, piernaLR['nuevo_femur'][i, 0], piernaLR['nuevo_tibia'][i, 0], piernaLR['nuevo_tarsus'][i, 0] ] lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[ind].positions = piernasLR_goalF01 # lr_trajectory1.points[ind].velocities = [0.0 for k in piernasLF_goalF01] # lr_trajectory1.points[ind].accelerations = [0.0 for k in piernasLF_goalF01] piernasRF_goalF01 = [ 0, -piernaRF['nuevo_femur'][i, 0], piernaRF['nuevo_tibia'][i, 0], piernaRF['nuevo_tarsus'][i, 0] ] rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[ind].positions = piernasRF_goalF01 # rf_trajectory1.points[ind].velocities = [0.0 for k in piernasLF_goalF01] # rf_trajectory1.points[ind].accelerations = [0.0 for k in piernasLF_goalF01] piernasRR_goalF01 = [ 0, -piernaRR['nuevo_femur'][i, 0], piernaRR['nuevo_tibia'][i, 0], piernaRR['nuevo_tarsus'][i, 0] ] rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[ind].positions = piernasRR_goalF01 # rr_trajectory1.points[ind].velocities = [0.0 for k in piernasLF_goalF01] # rr_trajectory1.points[ind].accelerations = [0.0 for k in piernasLF_goalF01] # print ind # print int(art1_der['senialCompleta'].shape[1]*0.2) # arm_trajectory.points[ind].velocities = [0.0 for j in piernas_goalF] # arm_trajectory.points[ind].accelerations = [0.0 for j in piernas_goalF] # art1_der['tiempo_completo'][0, i] # 0.01 * (ind + 1) lf_trajectory1.points[ind].time_from_start = rospy.Duration( 1 + (piernaLF['tiempo'][i, 0] - piernaLF['tiempo'][0, 0]) * 2.8) lr_trajectory1.points[ind].time_from_start = rospy.Duration( 1 + (piernaLR['tiempo'][i, 0] - piernaLR['tiempo'][0, 0]) * 2.8) rf_trajectory1.points[ind].time_from_start = rospy.Duration( 1 + (piernaRF['tiempo'][i, 0] - piernaRF['tiempo'][0, 0]) * 2.8) rr_trajectory1.points[ind].time_from_start = rospy.Duration( 1 + (piernaRR['tiempo'][i, 0] - piernaRR['tiempo'][0, 0]) * 2.8) # print 1+art1_der['tiempo_completo'][0, i] ind += 1 # print art1_der['senialCompleta'][0, i] print int(piernaRR['nuevo_tarsus'].shape[0]) lf_reposo_trajectory1 = JointTrajectory() lf_reposo_trajectory1.joint_names = piernas_joints_LF lf_reposo_trajectory1.points.append(JointTrajectoryPoint()) lf_reposo_trajectory1.points[0].positions = [ 0.0 for i in piernas_joints_RF ] lf_reposo_trajectory1.points[0].velocities = [ 0.0 for i in piernas_joints_RF ] lf_reposo_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] lf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2) lr_reposo_trajectory1 = JointTrajectory() lr_reposo_trajectory1.joint_names = piernas_joints_LR lr_reposo_trajectory1.points.append(JointTrajectoryPoint()) lr_reposo_trajectory1.points[0].positions = [ 0.0 for i in piernas_joints_RF ] lr_reposo_trajectory1.points[0].velocities = [ 0.0 for i in piernas_joints_RF ] lr_reposo_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] lr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2) rr_reposo_trajectory1 = JointTrajectory() rr_reposo_trajectory1.joint_names = piernas_joints_RR rr_reposo_trajectory1.points.append(JointTrajectoryPoint()) rr_reposo_trajectory1.points[0].positions = [ 0.0 for i in piernas_joints_RF ] rr_reposo_trajectory1.points[0].velocities = [ 0.0 for i in piernas_joints_RF ] rr_reposo_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] rr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2) rf_reposo_trajectory1 = JointTrajectory() rf_reposo_trajectory1.joint_names = piernas_joints_RF rf_reposo_trajectory1.points.append(JointTrajectoryPoint()) rf_reposo_trajectory1.points[0].positions = [ 0.0 for i in piernas_joints_RF ] rf_reposo_trajectory1.points[0].velocities = [ 0.0 for i in piernas_joints_RF ] rf_reposo_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] rf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2) # Create an empty trajectory goal rospy.loginfo('Moving the arm to goal position...') # Create an empty trajectory goal rr_goal = FollowJointTrajectoryGoal() lm_goal = FollowJointTrajectoryGoal() rf_goal = FollowJointTrajectoryGoal() lr_goal = FollowJointTrajectoryGoal() rm_goal = FollowJointTrajectoryGoal() lf_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above rr_goal.trajectory = rr_trajectory1 rf_goal.trajectory = rf_trajectory1 lr_goal.trajectory = lr_trajectory1 lf_goal.trajectory = lf_trajectory1 # Specify zero tolerance for the execution time rr_goal.goal_time_tolerance = rospy.Duration(0.01) lm_goal.goal_time_tolerance = rospy.Duration(0.01) rf_goal.goal_time_tolerance = rospy.Duration(0.01) lr_goal.goal_time_tolerance = rospy.Duration(0.01) rm_goal.goal_time_tolerance = rospy.Duration(0.01) lf_goal.goal_time_tolerance = rospy.Duration(0.01) # Send the goal to the action server rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) rr_client.wait_for_result() '''rr_client.send_goal(rr_goal) lm_client.send_goal(lm_goal) rf_client.send_goal(rf_goal) lr_client.send_goal(lr_goal) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) rr_client.wait_for_result() lr_client.send_goal(lr_goal) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) rr_client.wait_for_result() rr_client.send_goal(rr_goal) lm_client.send_goal(lm_goal) rf_client.send_goal(rf_goal) lr_client.send_goal(lr_goal) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) rr_client.wait_for_result() #''' rr_goal.trajectory = rr_reposo_trajectory1 rf_goal.trajectory = rf_reposo_trajectory1 lr_goal.trajectory = lr_reposo_trajectory1 lf_goal.trajectory = lf_reposo_trajectory1 rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rospy.loginfo('...done')
def marker_inf_cb(self, data): # in order to filter the inamissible (0.0, 0.0, 0.0, -inf) detections if data.data[0:3] == (0.0, 0.0, 0.0): # print "inadmissible input!" return vec3 = Vector3() vec3.x = data.data[0] vec3.y = data.data[1] vec3.z = data.data[2] ball_pose = np.array([vec3.x, vec3.y, vec3.z]) for i in range(3): if np.isnan(ball_pose[i]): return cam_object_T = self.set_cam_object_T(ball_pose) cam_object_R = tr.identity_matrix() cam_object_R[0:3, 0:3] = cam_object_T[0:3, 0:3] cam_object_q = tr.quaternion_from_matrix(cam_object_R) self.br.sendTransform( (cam_object_T[0, 3], cam_object_T[1, 3], cam_object_T[2, 3]), cam_object_q, rospy.Time.now(), "object_in_cam_frame", "camera_rgb_optical_frame") # publishing the object marker to rviz for object_in_cam det_marker = Marker() det_marker.header.frame_id = "camera_rgb_optical_frame" det_marker.header.stamp = rospy.Time.now() det_marker.type = Marker.SPHERE det_marker.pose.position.x = cam_object_T[0, 3] det_marker.pose.position.y = cam_object_T[1, 3] det_marker.pose.position.z = cam_object_T[2, 3] det_marker.pose.orientation.x = 0 det_marker.pose.orientation.y = 0 det_marker.pose.orientation.z = 0 det_marker.pose.orientation.w = 1 det_marker.scale.x = 0.05 det_marker.scale.y = 0.05 det_marker.scale.z = 0.05 det_marker.color.a = 1.0 det_marker.color.r = 1.0 det_marker.color.g = 0.0 det_marker.color.b = 0.0 self.marker_object_in_cam_pub.publish(det_marker) base_object_T = np.dot(np.dot(self.base_end_T, self.end_cam_T), cam_object_T) # in order to assign object the same orientation as target frame base_object_T[0:3, 0:3] = self.base_target_T[0:3, 0:3] final_err = self.error_computation(base_object_T, self.base_target_T) # target thresholds: trans_th = [0.05, 0.05, 0.05] rot_th = [15 * np.pi / 180, 15 * np.pi / 180, 15 * np.pi / 180] print "timer: ", self.timer if self.timer > 5: print "goal reached ............................" self.gripper_final_action() if np.abs(final_err[0]) < trans_th[0] and np.abs( final_err[1]) < trans_th[1] and np.abs( final_err[2]) < trans_th[2]: if np.abs(final_err[3]) < rot_th[0] and np.abs( final_err[4]) < rot_th[1] and np.abs( final_err[5]) < rot_th[2]: self.timer += 1 return else: self.timer = 0 print "error :", final_err else: self.timer = 0 print "error :", final_err # use of error_gains: error_gains = np.reshape(np.array([1, 1, 1, 1, 1, 1]), (6, 1)) final_err2 = np.multiply(final_err, error_gains) # error value type 2 end_object_T = np.dot(self.end_cam_T, cam_object_T) end_target_T = np.dot(self.end_grip_T, self.grip_target_T) object_target_T = np.dot(end_object_T, np.linalg.inv(end_target_T)) # print "error style 2: ", object_target_T # publishing the object marker to rviz det_marker = Marker() det_marker.header.frame_id = "iiwa_base_link" det_marker.header.stamp = rospy.Time.now() det_marker.type = Marker.SPHERE det_marker.pose.position.x = base_object_T[0, 3] det_marker.pose.position.y = base_object_T[1, 3] det_marker.pose.position.z = base_object_T[2, 3] det_marker.pose.orientation.x = 0 det_marker.pose.orientation.y = 0 det_marker.pose.orientation.z = 0 det_marker.pose.orientation.w = 1 det_marker.scale.x = 0.05 det_marker.scale.y = 0.05 det_marker.scale.z = 0.05 det_marker.color.a = 1.0 det_marker.color.r = 0.0 det_marker.color.g = 0.0 det_marker.color.b = 1.0 self.marker_object_in_base_pub.publish(det_marker) # print "final error is : ", final_err # print "base_target_T: ", base_object_T # computation of jacobian of the manipulator J = self.kdl_kin.jacobian(self.end_eff_current_pose) if self.used_method == "DLSM": # use of damped_least_squares as matrix inverse J_inverse = np.dot( J.T, np.linalg.inv( np.dot(J, J.T) + ((self.landa**2) * np.identity(6)))) elif self.used_method == "JPM": # use of matrix pseudo_inverse as matrix inverse J_inverse = np.linalg.pinv(J) elif self.used_method == "JTM": # use of matrix transpose as matrix inverse J_inverse = np.transpose(J) final_vel = np.dot(J_inverse, final_err2) # the dereived final speed command for the manipulator speed_cmd = list(np.array(final_vel).reshape(-1, )) # avoid huge joint velocities: joint_vel_th = [0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7] if speed_cmd[0] > joint_vel_th[0]: print "huge velocity for the first joint!" return elif speed_cmd[1] > joint_vel_th[1]: print "huge velocity for the second joint!" return elif speed_cmd[2] > joint_vel_th[2]: print "huge velocity for the third joint!" return elif speed_cmd[3] > joint_vel_th[3]: print "huge velocity for the fourth joint!" return elif speed_cmd[4] > joint_vel_th[4]: print "huge velocity for the fifth joint!" return elif speed_cmd[5] > joint_vel_th[5]: print "huge velocity for the sixth joint!" return elif speed_cmd[6] > joint_vel_th[6]: print "huge velocity for the seventh joint!" return # block to handle speed publisher using pose publisher intergration_time_step = 0.3 pose_change = [speed * intergration_time_step for speed in speed_cmd] final_pose = [ a + b for a, b in zip(self.end_eff_current_pose, pose_change) ] # actionlib commands self.trajectory.points[0].positions = final_pose self.trajectory.points[0].velocities = [0.0] * len(self.joint_names) self.trajectory.points[0].accelerations = [0.0] * len(self.joint_names) self.trajectory.points[0].time_from_start = rospy.Duration(1.0) # print "pose_change", pose_change # print "The manipulator is moving..." goal = FollowJointTrajectoryGoal() goal.trajectory = self.trajectory goal.goal_time_tolerance = rospy.Duration(0.0) self.manipulator_client.send_goal(goal) action_result = self.manipulator_client.wait_for_result() if action_result is True: # publishing the path of the object in "red" color final_object_position = base_object_T[0:3, 3] final_object_R = tr.identity_matrix() final_object_R[0:3, 0:3] = base_object_T[0:3, 0:3] final_object_quaternion = tr.quaternion_from_matrix(final_object_R) self.object_path.markers.append(Marker()) self.object_path.markers[-1].header.frame_id = "iiwa_base_link" self.object_path.markers[-1].header.stamp = rospy.Time.now() self.object_path.markers[-1].id = len(self.object_path.markers) self.object_path.markers[-1].type = Marker.SPHERE self.object_path.markers[ -1].pose.position.x = final_object_position[0] self.object_path.markers[ -1].pose.position.y = final_object_position[1] self.object_path.markers[ -1].pose.position.z = final_object_position[2] self.object_path.markers[ -1].pose.orientation.x = final_object_quaternion[0] self.object_path.markers[ -1].pose.orientation.y = final_object_quaternion[1] self.object_path.markers[ -1].pose.orientation.z = final_object_quaternion[2] self.object_path.markers[ -1].pose.orientation.w = final_object_quaternion[3] self.object_path.markers[-1].scale.x = 0.01 self.object_path.markers[-1].scale.y = 0.01 self.object_path.markers[-1].scale.z = 0.01 self.object_path.markers[-1].color.a = 1.0 self.object_path.markers[-1].color.r = 1.0 self.object_path.markers[-1].color.g = 0.0 self.object_path.markers[-1].color.b = 0.0 self.marker_obj_path_pub.publish(self.object_path) # publishing path of the end effector in "green" color final_ee_position = self.base_end_T[0:3, 3] final_ee_R = tr.identity_matrix() final_ee_R[0:3, 0:3] = self.base_end_T[0:3, 0:3] final_ee_quaternion = tr.quaternion_from_matrix(final_ee_R) self.ee_path.markers.append(Marker()) self.ee_path.markers[-1].header.frame_id = "iiwa_base_link" self.ee_path.markers[-1].header.stamp = rospy.Time.now() self.ee_path.markers[-1].id = len(self.ee_path.markers) self.ee_path.markers[-1].type = Marker.SPHERE self.ee_path.markers[-1].pose.position.x = final_ee_position[0] self.ee_path.markers[-1].pose.position.y = final_ee_position[1] self.ee_path.markers[-1].pose.position.z = final_ee_position[2] self.ee_path.markers[-1].pose.orientation.x = final_ee_quaternion[ 0] self.ee_path.markers[-1].pose.orientation.y = final_ee_quaternion[ 1] self.ee_path.markers[-1].pose.orientation.z = final_ee_quaternion[ 2] self.ee_path.markers[-1].pose.orientation.w = final_ee_quaternion[ 3] self.ee_path.markers[-1].scale.x = 0.01 self.ee_path.markers[-1].scale.y = 0.01 self.ee_path.markers[-1].scale.z = 0.01 self.ee_path.markers[-1].color.a = 1.0 self.ee_path.markers[-1].color.r = 0.0 self.ee_path.markers[-1].color.g = 1.0 self.ee_path.markers[-1].color.b = 0.0 self.marker_eff_path_pub.publish(self.ee_path)
def run(self, joint_pose, duration=2.0): """ The run function for this step Args: duration (float) : The amount of time within which to execute the trajectory joint_pose (str, list, tuple, dict) : The arm poses to move to. If the type is: * str. Then if the string starts with * `joint_poses`, get a ``assistance_msgs/ArmJointPose`` \ from :const:`ARM_JOINT_POSES_SERVICE_NAME` and move the \ joints to the desired pose * list, tuple. Then if the list is of * `floats`, move the joints to the desired pose indicated \ with the float values .. seealso:: :meth:`task_executor.abstract_step.AbstractStep.run` """ # Parse out the pose parsed_pose = self._parse_pose(joint_pose) if parsed_pose is None: rospy.logerr("Action {}: FAIL. Unknown Format: {}".format(self.name, joint_pose)) raise KeyError(self.name, "Unknown Format", joint_pose) rospy.loginfo("Action {}: Moving arm to pose {}".format(self.name, parsed_pose)) # Create and send the goal goal = FollowJointTrajectoryGoal() trajectory = JointTrajectory() trajectory.joint_names = ArmFollowJointTrajAction.JOINT_NAMES trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = parsed_pose trajectory.points[0].velocities = [0.0 for _ in parsed_pose] trajectory.points[0].accelerations = [0.0 for _ in parsed_pose] trajectory.points[0].time_from_start = rospy.Duration(duration) goal.trajectory = trajectory self._arm_follow_joint_traj_client.send_goal(goal) # Yield an empty dict while we're executing while self._arm_follow_joint_traj_client.get_state() in AbstractStep.RUNNING_GOAL_STATES: yield self.set_running() # Wait for a result and yield based on how we exited status = self._arm_follow_joint_traj_client.get_state() self._arm_follow_joint_traj_client.wait_for_result() result = self._arm_follow_joint_traj_client.get_result() if status == GoalStatus.SUCCEEDED: yield self.set_succeeded() elif status == GoalStatus.PREEMPTED: yield self.set_preempted( action=self.name, status=status, result=result, ) else: yield self.set_aborted( action=self.name, status=status, result=result, )
def __init__(self): filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/mi_cuadrupedo_exp.urdf' angulo_avance = +0.4 #rad altura_pata = +0.06 #cm avance_x = 0.03 # angulo_avance = 0 # +0.40 #rad # altura_pata = 0 # -0.04 #cm avance_x = 0.11 robot = urdf_parser_py.urdf.URDF.from_xml_file(filename) (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) cadena_RR = tree.getChain("base_link", "tarsus_RR") cadena_RF = tree.getChain("base_link", "tarsus_RF") cadena_LR = tree.getChain("base_link", "tarsus_LR") cadena_LF = tree.getChain("base_link", "tarsus_LF") print cadena_RR.getNrOfSegments() fksolver_RR = PyKDL.ChainFkSolverPos_recursive(cadena_RR) fksolver_RF = PyKDL.ChainFkSolverPos_recursive(cadena_RF) fksolver_LR = PyKDL.ChainFkSolverPos_recursive(cadena_LR) fksolver_LF = PyKDL.ChainFkSolverPos_recursive(cadena_LF) vik_RR = PyKDL.ChainIkSolverVel_pinv(cadena_RR) ik_RR = PyKDL.ChainIkSolverPos_NR(cadena_RR, fksolver_RR, vik_RR) vik_RF = PyKDL.ChainIkSolverVel_pinv(cadena_RF) ik_RF = PyKDL.ChainIkSolverPos_NR(cadena_RF, fksolver_RF, vik_RF) vik_LR = PyKDL.ChainIkSolverVel_pinv(cadena_LR) ik_LR = PyKDL.ChainIkSolverPos_NR(cadena_LR, fksolver_LR, vik_LR) vik_LF = PyKDL.ChainIkSolverVel_pinv(cadena_LF) ik_LF = PyKDL.ChainIkSolverPos_NR(cadena_LF, fksolver_LF, vik_LF) nj_izq = cadena_RR.getNrOfJoints() posicionInicial_R = PyKDL.JntArray(nj_izq) posicionInicial_R[0] = 0 posicionInicial_R[1] = 0 posicionInicial_R[2] = 0 posicionInicial_R[3] = 0 nj_izq = cadena_LR.getNrOfJoints() posicionInicial_L = PyKDL.JntArray(nj_izq) posicionInicial_L[0] = 0 posicionInicial_L[1] = 0 posicionInicial_L[2] = 0 posicionInicial_L[3] = 0 print "Forward kinematics" finalFrame_R = PyKDL.Frame() finalFrame_L = PyKDL.Frame() rospy.init_node('trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set to False to wait for arm to finish before moving head sync = rospy.get_param('~sync', True) # Which joints define the arm? piernas_joints_RR = [ 'coxa_joint_RR', 'femur_joint_RR', 'tibia_joint_RR', 'tarsus_joint_RR' ] piernas_joints_RF = [ 'coxa_joint_RF', 'femur_joint_RF', 'tibia_joint_RF', 'tarsus_joint_RF' ] piernas_joints_LR = [ 'coxa_joint_LR', 'femur_joint_LR', 'tibia_joint_LR', 'tarsus_joint_LR' ] piernas_joints_LF = [ 'coxa_joint_LF', 'femur_joint_LF', 'tibia_joint_LF', 'tarsus_joint_LF' ] rr_goal0 = [0.0, 0.0, 0.0, 0.0] rf_goal0 = [0.0, 0.0, 0.0, 0.0] rr_goal1 = [0.0, 0.0, 0.0, 0.0] rf_goal1 = [0.0, 0.0, 0.0, 0.0] lr_goal0 = [0.0, 0.0, 0.0, 0.0] lf_goal0 = [0.0, 0.0, 0.0, 0.0] lr_goal1 = [0.0, 0.0, 0.0, 0.0] lf_goal1 = [0.0, 0.0, 0.0, 0.0] #11111111111111111111111111111111111111111111 print "Inverse Kinematics" fksolver_RR.JntToCart(posicionInicial_R, finalFrame_R) q_init_RR = posicionInicial_R # initial angles desiredFrameRR = finalFrame_R desiredFrameRR.p[0] = finalFrame_R.p[0] + avance_x desiredFrameRR.p[1] = finalFrame_R.p[1] desiredFrameRR.p[2] = finalFrame_R.p[2] + altura_pata print "Desired Position: ", desiredFrameRR.p q_out_RR = PyKDL.JntArray(cadena_RR.getNrOfJoints()) ik_RR.CartToJnt(q_init_RR, desiredFrameRR, q_out_RR) print "Output angles in rads: ", q_out_RR rr_goal0[0] = q_out_RR[0] rr_goal0[1] = q_out_RR[1] #+angulo_avance rr_goal0[2] = q_out_RR[2] rr_goal0[3] = q_out_RR[3] print "Inverse Kinematics" fksolver_LF.JntToCart(posicionInicial_L, finalFrame_L) q_init_LF = posicionInicial_L # initial angles desiredFrameLF = finalFrame_L desiredFrameLF.p[0] = finalFrame_L.p[0] + avance_x desiredFrameLF.p[1] = finalFrame_L.p[1] desiredFrameLF.p[2] = finalFrame_L.p[2] + altura_pata print "Desired Position: ", desiredFrameLF.p q_out_LF = PyKDL.JntArray(cadena_LF.getNrOfJoints()) ik_LF.CartToJnt(q_init_LF, desiredFrameLF, q_out_LF) print "Output angles in rads: ", q_out_LF lf_goal0[0] = q_out_LF[0] lf_goal0[1] = q_out_LF[1] #- angulo_avance lf_goal0[2] = q_out_LF[2] lf_goal0[3] = q_out_LF[3] # 22222222222222222222222222222222222222222222 RR_actual = PyKDL.JntArray(nj_izq) RR_actual[0] = rr_goal0[0] RR_actual[1] = rr_goal0[1] RR_actual[2] = rr_goal0[2] RR_actual[3] = rr_goal0[3] print "Inverse Kinematics" fksolver_RR.JntToCart(RR_actual, finalFrame_R) q_init_RR = RR_actual # initial angles desiredFrameRR = finalFrame_R desiredFrameRR.p[0] = finalFrame_R.p[0] - avance_x desiredFrameRR.p[1] = finalFrame_R.p[1] desiredFrameRR.p[2] = finalFrame_R.p[2] - altura_pata print "Desired Position: ", desiredFrameRR.p q_out_RR = PyKDL.JntArray(cadena_RR.getNrOfJoints()) ik_RR.CartToJnt(q_init_RR, desiredFrameRR, q_out_RR) print "Output angles in rads: ", q_out_RR rr_goal1[0] = q_out_RR[0] rr_goal1[1] = q_out_RR[1] rr_goal1[2] = q_out_RR[2] rr_goal1[3] = q_out_RR[3] LF_actual = PyKDL.JntArray(nj_izq) LF_actual[0] = lf_goal0[0] LF_actual[1] = lf_goal0[1] LF_actual[2] = lf_goal0[2] LF_actual[3] = lf_goal0[3] print "Inverse Kinematics" fksolver_LF.JntToCart(LF_actual, finalFrame_L) q_init_LF = LF_actual # initial angles desiredFrameLF = finalFrame_L desiredFrameLF.p[0] = finalFrame_L.p[0] - avance_x desiredFrameLF.p[1] = finalFrame_L.p[1] desiredFrameLF.p[2] = finalFrame_L.p[2] - altura_pata print "Desired Position: ", desiredFrameLF.p q_out_LF = PyKDL.JntArray(cadena_LF.getNrOfJoints()) ik_LF.CartToJnt(q_init_LF, desiredFrameLF, q_out_LF) print "Output angles in rads: ", q_out_LF lf_goal1[0] = q_out_LF[0] lf_goal1[1] = q_out_LF[1] # - angulo_avance lf_goal1[2] = q_out_LF[2] lf_goal1[3] = q_out_LF[3] # 11111111111111111111111111111111111111111111 print "Inverse Kinematics" fksolver_LR.JntToCart(posicionInicial_L, finalFrame_L) q_init_LR = posicionInicial_L # initial angles desiredFrameLR = finalFrame_L desiredFrameLR.p[0] = finalFrame_L.p[0] #-avance_x desiredFrameLR.p[1] = finalFrame_L.p[1] desiredFrameLR.p[2] = finalFrame_L.p[2] # + altura_pata print "Desired Position: ", desiredFrameLR.p q_out_LR = PyKDL.JntArray(cadena_LR.getNrOfJoints()) ik_LR.CartToJnt(q_init_LR, desiredFrameLR, q_out_LR) print "Output angles in rads: ", q_out_LR lr_goal0[0] = q_out_LR[0] lr_goal0[1] = q_out_LR[1] + angulo_avance lr_goal0[2] = q_out_LR[2] lr_goal0[3] = q_out_LR[3] print "Inverse Kinematics" fksolver_RF.JntToCart(posicionInicial_R, finalFrame_R) q_init_RF = posicionInicial_R # initial angles desiredFrameRF = finalFrame_R desiredFrameRF.p[0] = finalFrame_R.p[0] # -avance_x desiredFrameRF.p[1] = finalFrame_R.p[1] desiredFrameRF.p[2] = finalFrame_R.p[2] #+ altura_pata print "Desired Position: ", desiredFrameRF.p q_out_RF = PyKDL.JntArray(cadena_RF.getNrOfJoints()) ik_RF.CartToJnt(q_init_RF, desiredFrameRF, q_out_RF) print "Output angles in rads: ", q_out_RF rf_goal0[0] = q_out_RF[0] rf_goal0[1] = q_out_RF[1] - angulo_avance rf_goal0[2] = q_out_RF[2] rf_goal0[3] = q_out_RF[3] # 2222222222222222222222222222222222222222222222 LR_actual = PyKDL.JntArray(nj_izq) LR_actual[0] = lr_goal0[0] LR_actual[1] = lr_goal0[1] LR_actual[2] = lr_goal0[2] LR_actual[3] = lr_goal0[3] print "Inverse Kinematics" fksolver_LR.JntToCart(LR_actual, finalFrame_L) q_init_LR = LR_actual # initial angles print "Inverse Kinematics" desiredFrameLR = finalFrame_L desiredFrameLR.p[0] = finalFrame_L.p[0] #+ avance_x desiredFrameLR.p[1] = finalFrame_L.p[1] desiredFrameLR.p[2] = finalFrame_L.p[2] # - altura_pata print "Desired Position: ", desiredFrameLR.p q_out_LR = PyKDL.JntArray(cadena_LR.getNrOfJoints()) ik_LR.CartToJnt(q_init_LR, desiredFrameLR, q_out_LR) print "Output angles in rads: ", q_out_LR lr_goal1[0] = q_out_LR[0] #- angulo_avance lr_goal1[1] = q_out_LR[1] lr_goal1[2] = q_out_LR[2] lr_goal1[3] = q_out_LR[3] + angulo_avance RF_actual = PyKDL.JntArray(nj_izq) RF_actual[0] = rf_goal0[0] RF_actual[1] = rf_goal0[1] RF_actual[2] = rf_goal0[2] RF_actual[3] = rf_goal0[3] print "Inverse Kinematics" fksolver_RF.JntToCart(RF_actual, finalFrame_R) q_init_RF = RF_actual # initial angles desiredFrameRF = finalFrame_R desiredFrameRF.p[0] = finalFrame_R.p[0] # + avance_x desiredFrameRF.p[1] = finalFrame_R.p[1] desiredFrameRF.p[2] = finalFrame_R.p[2] #- altura_pata print "Desired Position: ", desiredFrameRF.p q_out_RF = PyKDL.JntArray(cadena_RF.getNrOfJoints()) ik_RF.CartToJnt(q_init_RF, desiredFrameRF, q_out_RF) print "Output angles in rads: ", q_out_RF rf_goal1[0] = q_out_RF[0] rf_goal1[1] = q_out_RF[1] rf_goal1[2] = q_out_RF[2] rf_goal1[3] = q_out_RF[3] + angulo_avance # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') rr_client = actionlib.SimpleActionClient( '/cuadrupedo/pata_rr/follow_joint_trajectory', FollowJointTrajectoryAction) rr_client.wait_for_server() rf_client = actionlib.SimpleActionClient( '/cuadrupedo/pata_rf/follow_joint_trajectory', FollowJointTrajectoryAction) rf_client.wait_for_server() lr_client = actionlib.SimpleActionClient( '/cuadrupedo/pata_lr/follow_joint_trajectory', FollowJointTrajectoryAction) lr_client.wait_for_server() lf_client = actionlib.SimpleActionClient( '/cuadrupedo/pata_lf/follow_joint_trajectory', FollowJointTrajectoryAction) lf_client.wait_for_server() rospy.loginfo('...connected.') # Create a single-point arm trajectory with the piernas_goal as the end-point rr_trajectory1 = JointTrajectory() rr_trajectory1.joint_names = piernas_joints_RR rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[0].positions = rr_goal0 rr_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RR] rr_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RR ] rr_trajectory1.points[0].time_from_start = rospy.Duration(0.2) rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[1].positions = rr_goal1 rr_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RR] rr_trajectory1.points[1].accelerations = [ 0.0 for i in piernas_joints_RR ] rr_trajectory1.points[1].time_from_start = rospy.Duration(0.4) rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[2].positions = rf_goal0 rr_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RR] rr_trajectory1.points[2].accelerations = [ 0.0 for i in piernas_joints_RR ] rr_trajectory1.points[2].time_from_start = rospy.Duration(0.6) rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[3].positions = rf_goal1 rr_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RR] rr_trajectory1.points[3].accelerations = [ 0.0 for i in piernas_joints_RR ] rr_trajectory1.points[3].time_from_start = rospy.Duration(0.8) #''' rf_trajectory1 = JointTrajectory() rf_trajectory1.joint_names = piernas_joints_RF rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[0].positions = rf_goal0 # [0,0,0,0] rf_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RF] rf_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] rf_trajectory1.points[0].time_from_start = rospy.Duration(0.2) rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[1].positions = rf_goal1 # [0,0,0,0] rf_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RF] rf_trajectory1.points[1].accelerations = [ 0.0 for i in piernas_joints_RF ] rf_trajectory1.points[1].time_from_start = rospy.Duration(0.4) rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[2].positions = rr_goal0 # [0,0,0,0] rf_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RF] rf_trajectory1.points[2].accelerations = [ 0.0 for i in piernas_joints_RF ] rf_trajectory1.points[2].time_from_start = rospy.Duration(0.6) rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[3].positions = rr_goal1 # [0,0,0,0] rf_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RF] rf_trajectory1.points[3].accelerations = [ 0.0 for i in piernas_joints_RF ] rf_trajectory1.points[3].time_from_start = rospy.Duration(0.8) # ''' lr_trajectory1 = JointTrajectory() lr_trajectory1.joint_names = piernas_joints_LR lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[0].positions = lr_goal0 #lr_goal0 lr_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RR] lr_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RR ] lr_trajectory1.points[0].time_from_start = rospy.Duration(0.2) lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[1].positions = lr_goal1 lr_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RR] lr_trajectory1.points[1].accelerations = [ 0.0 for i in piernas_joints_RR ] lr_trajectory1.points[1].time_from_start = rospy.Duration(0.4) lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[2].positions = lf_goal0 lr_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RR] lr_trajectory1.points[2].accelerations = [ 0.0 for i in piernas_joints_RR ] lr_trajectory1.points[2].time_from_start = rospy.Duration(0.6) lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[3].positions = lf_goal1 # lr_goal0 lr_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RR] lr_trajectory1.points[3].accelerations = [ 0.0 for i in piernas_joints_RR ] lr_trajectory1.points[3].time_from_start = rospy.Duration(0.8) # ''' lf_trajectory1 = JointTrajectory() lf_trajectory1.joint_names = piernas_joints_LF lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[0].positions = lf_goal0 #lf_goal0 # [0,0,0,0] lf_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RF] lf_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] lf_trajectory1.points[0].time_from_start = rospy.Duration(0.2) lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[1].positions = lf_goal1 # [0,0,0,0] lf_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RF] lf_trajectory1.points[1].accelerations = [ 0.0 for i in piernas_joints_RF ] lf_trajectory1.points[1].time_from_start = rospy.Duration(0.4) lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[2].positions = lr_goal0 # [0,0,0,0] lf_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RF] lf_trajectory1.points[2].accelerations = [ 0.0 for i in piernas_joints_RF ] lf_trajectory1.points[2].time_from_start = rospy.Duration(0.6) lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[3].positions = lr_goal1 # lf_goal0 # [0,0,0,0] lf_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RF] lf_trajectory1.points[3].accelerations = [ 0.0 for i in piernas_joints_RF ] lf_trajectory1.points[3].time_from_start = rospy.Duration(0.8) # ''' # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') lf_reposo_trajectory1 = JointTrajectory() lf_reposo_trajectory1.joint_names = piernas_joints_LF lf_reposo_trajectory1.points.append(JointTrajectoryPoint()) lf_reposo_trajectory1.points[0].positions = [ 0.0 for i in piernas_joints_RF ] lf_reposo_trajectory1.points[0].velocities = [ 0.0 for i in piernas_joints_RF ] lf_reposo_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] lf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2) lr_reposo_trajectory1 = JointTrajectory() lr_reposo_trajectory1.joint_names = piernas_joints_LR lr_reposo_trajectory1.points.append(JointTrajectoryPoint()) lr_reposo_trajectory1.points[0].positions = [ 0.0 for i in piernas_joints_RF ] lr_reposo_trajectory1.points[0].velocities = [ 0.0 for i in piernas_joints_RF ] lr_reposo_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] lr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2) rr_reposo_trajectory1 = JointTrajectory() rr_reposo_trajectory1.joint_names = piernas_joints_RR rr_reposo_trajectory1.points.append(JointTrajectoryPoint()) rr_reposo_trajectory1.points[0].positions = [ 0.0 for i in piernas_joints_RF ] rr_reposo_trajectory1.points[0].velocities = [ 0.0 for i in piernas_joints_RF ] rr_reposo_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] rr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2) rf_reposo_trajectory1 = JointTrajectory() rf_reposo_trajectory1.joint_names = piernas_joints_RF rf_reposo_trajectory1.points.append(JointTrajectoryPoint()) rf_reposo_trajectory1.points[0].positions = [ 0.0 for i in piernas_joints_RF ] rf_reposo_trajectory1.points[0].velocities = [ 0.0 for i in piernas_joints_RF ] rf_reposo_trajectory1.points[0].accelerations = [ 0.0 for i in piernas_joints_RF ] rf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2) # Create an empty trajectory goal rr_goal = FollowJointTrajectoryGoal() lm_goal = FollowJointTrajectoryGoal() rf_goal = FollowJointTrajectoryGoal() lr_goal = FollowJointTrajectoryGoal() rm_goal = FollowJointTrajectoryGoal() lf_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above rr_goal.trajectory = rr_trajectory1 rf_goal.trajectory = rf_trajectory1 lr_goal.trajectory = lr_trajectory1 lf_goal.trajectory = lf_trajectory1 # Specify zero tolerance for the execution time rr_goal.goal_time_tolerance = rospy.Duration(0.0) lm_goal.goal_time_tolerance = rospy.Duration(0.0) rf_goal.goal_time_tolerance = rospy.Duration(0.0) lr_goal.goal_time_tolerance = rospy.Duration(0.0) rm_goal.goal_time_tolerance = rospy.Duration(0.0) lf_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal to the action server rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) #rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) #lr_client.wait_for_result(rospy.Duration(5.0)) #rr_client.send_goal(rr_goal) #lm_client.send_goal(lm_goal) #rf_client.send_goal(rf_goal)''' if not sync: # Wait for up to 5 seconds for the motion to complete rr_client.wait_for_result(rospy.Duration(5.0)) rr_client.wait_for_result() print rr_client.get_result() rr_goal.trajectory = rr_reposo_trajectory1 rf_goal.trajectory = rf_reposo_trajectory1 lr_goal.trajectory = lr_reposo_trajectory1 lf_goal.trajectory = lf_reposo_trajectory1 rr_client.send_goal(rr_goal) rf_client.send_goal(rf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) print 'Resultado recibido' lr_client.send_goal(lr_goal) lf_client.send_goal(lf_goal) rr_client.wait_for_result() rospy.loginfo('...done')
def __init__(self): x = loadmat('hex_wave.mat' ) #hex_transversal hex_lateral2 hex_lateral_r hex_wave # print x pierna1 = x['pierna1'] pierna2 = x['pierna2'] pierna3 = x['pierna3'] pierna4 = x['pierna4'] pierna5 = x['pierna5'] pierna6 = x['pierna6'] rospy.init_node('trajectory_hex') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set to False to wait for arm to finish before moving head sync = rospy.get_param('~sync', True) # Which joints define the arm? piernas_joints_RR = [ 'coxa_joint_RR', 'femur_joint_RR', 'tibia_joint_RR', 'tarsus_joint_RR' ] piernas_joints_LM = [ 'coxa_joint_LM', 'femur_joint_LM', 'tibia_joint_LM', 'tarsus_joint_LM' ] piernas_joints_RF = [ 'coxa_joint_RF', 'femur_joint_RF', 'tibia_joint_RF', 'tarsus_joint_RF' ] piernas_joints_LR = [ 'coxa_joint_LR', 'femur_joint_LR', 'tibia_joint_LR', 'tarsus_joint_LR' ] piernas_joints_RM = [ 'coxa_joint_RM', 'femur_joint_RM', 'tibia_joint_RM', 'tarsus_joint_RM' ] piernas_joints_LF = [ 'coxa_joint_LF', 'femur_joint_LF', 'tibia_joint_LF', 'tarsus_joint_LF' ] rospy.loginfo('Waiting for right arm trajectory controller...') rr_client = actionlib.SimpleActionClient( '/hexapodo/pata_rr/follow_joint_trajectory', FollowJointTrajectoryAction) rr_client.wait_for_server() lm_client = actionlib.SimpleActionClient( '/hexapodo/pata_lm/follow_joint_trajectory', FollowJointTrajectoryAction) lm_client.wait_for_server() rf_client = actionlib.SimpleActionClient( '/hexapodo/pata_rf/follow_joint_trajectory', FollowJointTrajectoryAction) rf_client.wait_for_server() lr_client = actionlib.SimpleActionClient( '/hexapodo/pata_lr/follow_joint_trajectory', FollowJointTrajectoryAction) lr_client.wait_for_server() rm_client = actionlib.SimpleActionClient( '/hexapodo/pata_rm/follow_joint_trajectory', FollowJointTrajectoryAction) rm_client.wait_for_server() lf_client = actionlib.SimpleActionClient( '/hexapodo/pata_lf/follow_joint_trajectory', FollowJointTrajectoryAction) lf_client.wait_for_server() rospy.loginfo('...connected.') rr_trajectory1 = JointTrajectory() rr_trajectory1.joint_names = piernas_joints_RR lm_trajectory1 = JointTrajectory() lm_trajectory1.joint_names = piernas_joints_LM rf_trajectory1 = JointTrajectory() rf_trajectory1.joint_names = piernas_joints_RF lr_trajectory1 = JointTrajectory() lr_trajectory1.joint_names = piernas_joints_LR rm_trajectory1 = JointTrajectory() rm_trajectory1.joint_names = piernas_joints_RM lf_trajectory1 = JointTrajectory() lf_trajectory1.joint_names = piernas_joints_LF # piernas_goalF0 = [0, -0.3, -0.4, 0.8, -0.3, -0.4, # 0, -0.4, -0.3, 0.6, +0.4, -0.3] # arm_trajectory.points.append(JointTrajectoryPoint()) # arm_trajectory.points[0].positions = piernas_goalF0 # arm_trajectory.points[0].velocities = [0.0 for k in piernas_goalF0] # arm_trajectory.points[0].accelerations = [0.0 for k in piernas_goalF0] # arm_trajectory.points[0].time_from_start = rospy.Duration(2) piernasLF_goalF01 = [ pierna6[0, 0], pierna6[1, 0], pierna6[2, 0], -0.4 ] #0.35 lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[0].positions = piernasLF_goalF01 lf_trajectory1.points[0].time_from_start = rospy.Duration(1) piernasLM_goalF01 = [ pierna5[0, 0], pierna5[1, 0], pierna5[2, 0], -0.4 ] #0.35 lm_trajectory1.points.append(JointTrajectoryPoint()) lm_trajectory1.points[0].positions = piernasLM_goalF01 lm_trajectory1.points[0].time_from_start = rospy.Duration(1) piernasLR_goalF01 = [ pierna4[0, 0], pierna4[1, 0], pierna4[2, 0], -0.4 ] #0.35 lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[0].positions = piernasLR_goalF01 lr_trajectory1.points[0].time_from_start = rospy.Duration(1) piernasRF_goalF01 = [ pierna3[0, 0], -pierna3[1, 0], -pierna3[2, 0], 0.4 ] #0.35 rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[0].positions = piernasRF_goalF01 rf_trajectory1.points[0].time_from_start = rospy.Duration(1) piernasRM_goalF01 = [ pierna2[0, 0], -pierna2[1, 0], -pierna2[2, 0], 0.4 ] #0.35 rm_trajectory1.points.append(JointTrajectoryPoint()) rm_trajectory1.points[0].positions = piernasRM_goalF01 rm_trajectory1.points[0].time_from_start = rospy.Duration(1) piernasRR_goalF01 = [ pierna1[0, 0], -pierna1[1, 0], -pierna1[2, 0], 0.4 ] #0.35 rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[0].positions = piernasRR_goalF01 rr_trajectory1.points[0].time_from_start = rospy.Duration(1) ind = 1 salto = 5 t_actual = 1 for i in range(1, int(pierna1.shape[1] * 0.25), salto): piernasLF_goalF01 = [ pierna6[0, i], pierna6[1, i], pierna6[2, i], -0.4 ] #0.35 lf_trajectory1.points.append(JointTrajectoryPoint()) lf_trajectory1.points[ind].positions = piernasLF_goalF01 lf_trajectory1.points[0].velocities = [ 0.0 for k in piernasLF_goalF01 ] lf_trajectory1.points[0].accelerations = [ 0.0 for k in piernasLF_goalF01 ] piernasLM_goalF01 = [ pierna5[0, i], pierna5[1, i], pierna5[2, i], -0.4 ] #0.35 lm_trajectory1.points.append(JointTrajectoryPoint()) lm_trajectory1.points[ind].positions = piernasLM_goalF01 lm_trajectory1.points[0].velocities = [ 0.0 for k in piernasLM_goalF01 ] lm_trajectory1.points[0].accelerations = [ 0.0 for k in piernasLM_goalF01 ] piernasLR_goalF01 = [ pierna4[0, i], pierna4[1, i], pierna4[2, i], -0.4 ] #0.35 lr_trajectory1.points.append(JointTrajectoryPoint()) lr_trajectory1.points[ind].positions = piernasLR_goalF01 lr_trajectory1.points[0].velocities = [ 0.0 for k in piernasLR_goalF01 ] lr_trajectory1.points[0].accelerations = [ 0.0 for k in piernasLR_goalF01 ] piernasRF_goalF01 = [ pierna3[0, i], -pierna3[1, i], -pierna3[2, i], 0.4 ] #0.35 rf_trajectory1.points.append(JointTrajectoryPoint()) rf_trajectory1.points[ind].positions = piernasRF_goalF01 rf_trajectory1.points[0].velocities = [ 0.0 for k in piernasRF_goalF01 ] rf_trajectory1.points[0].accelerations = [ 0.0 for k in piernasRF_goalF01 ] piernasRM_goalF01 = [ pierna2[0, i], -pierna2[1, 0], -pierna2[2, i], 0.4 ] #0.35 rm_trajectory1.points.append(JointTrajectoryPoint()) rm_trajectory1.points[ind].positions = piernasRM_goalF01 rm_trajectory1.points[0].velocities = [ 0.0 for k in piernasRM_goalF01 ] rm_trajectory1.points[0].accelerations = [ 0.0 for k in piernasRM_goalF01 ] piernasRR_goalF01 = [ pierna1[0, i], -pierna1[1, 0], -pierna1[2, i], 0.4 ] #0.35 rr_trajectory1.points.append(JointTrajectoryPoint()) rr_trajectory1.points[ind].positions = piernasRR_goalF01 rr_trajectory1.points[0].velocities = [ 0.0 for k in piernasRR_goalF01 ] rr_trajectory1.points[0].accelerations = [ 0.0 for k in piernasRR_goalF01 ] # print ind # print int(art1_der['senialCompleta'].shape[1]*0.2) # arm_trajectory.points[ind].velocities = [0.0 for j in piernas_goalF] # arm_trajectory.points[ind].accelerations = [0.0 for j in piernas_goalF] # art1_der['tiempo_completo'][0, i] # 0.01 * (ind + 1) t_actual += 0.01 lf_trajectory1.points[ind].time_from_start = rospy.Duration( t_actual) lm_trajectory1.points[ind].time_from_start = rospy.Duration( t_actual) lr_trajectory1.points[ind].time_from_start = rospy.Duration( t_actual) rf_trajectory1.points[ind].time_from_start = rospy.Duration( t_actual) rm_trajectory1.points[ind].time_from_start = rospy.Duration( t_actual) rr_trajectory1.points[ind].time_from_start = rospy.Duration( t_actual) # print 1+art1_der['tiempo_completo'][0, i] ind += 1 # print art1_der['senialCompleta'][0, i] # Create an empty trajectory goal rospy.loginfo('Moving the arm to goal position...') print pierna1.shape[1] # Create an empty trajectory goal rr_goal = FollowJointTrajectoryGoal() lm_goal = FollowJointTrajectoryGoal() rf_goal = FollowJointTrajectoryGoal() lr_goal = FollowJointTrajectoryGoal() rm_goal = FollowJointTrajectoryGoal() lf_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above rr_goal.trajectory = rr_trajectory1 lm_goal.trajectory = lm_trajectory1 rf_goal.trajectory = rf_trajectory1 lr_goal.trajectory = lr_trajectory1 rm_goal.trajectory = rm_trajectory1 lf_goal.trajectory = lf_trajectory1 # Specify zero tolerance for the execution time rr_goal.goal_time_tolerance = rospy.Duration(0.01) lm_goal.goal_time_tolerance = rospy.Duration(0.01) rf_goal.goal_time_tolerance = rospy.Duration(0.01) lr_goal.goal_time_tolerance = rospy.Duration(0.01) rm_goal.goal_time_tolerance = rospy.Duration(0.01) lf_goal.goal_time_tolerance = rospy.Duration(0.01) # Send the goal to the action server rr_client.send_goal(rr_goal) lm_client.send_goal(lm_goal) rf_client.send_goal(rf_goal) lr_client.send_goal(lr_goal) rm_client.send_goal(rm_goal) lf_client.send_goal(lf_goal) # rr_client.wait_for_result(rospy.Duration(5.0)) rr_client.wait_for_result() # rr_client.send_goal(rr_goal) # lm_client.send_goal(lm_goal) # rf_client.send_goal(rf_goal) # # lr_client.send_goal(lr_goal) # rm_client.send_goal(rm_goal) # lf_client.send_goal(lf_goal) # # rr_client.wait_for_result(rospy.Duration(5.0)) # rr_client.wait_for_result() # # lr_client.send_goal(lr_goal) # rm_client.send_goal(rm_goal) # lf_client.send_goal(lf_goal) # # rr_client.wait_for_result(rospy.Duration(5.0)) # rr_client.wait_for_result() # # rr_client.send_goal(rr_goal) # lm_client.send_goal(lm_goal) # rf_client.send_goal(rf_goal) # # lr_client.send_goal(lr_goal) # rm_client.send_goal(rm_goal) # lf_client.send_goal(lf_goal) # # rr_client.wait_for_result(rospy.Duration(5.0)) # rr_client.wait_for_result() #''' rospy.loginfo('...done')
def gspline_to_follow_joint_trajectory_goal(_trj, _joint_names, _sample_rate): trj_msg = gspline_to_joint_trajectory_message(_trj, _joint_names, _sample_rate) msg = FollowJointTrajectoryGoal() msg.trajectory = trj_msg return msg
def __init__(self): filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado3.urdf' robot = urdf_parser_py.urdf.URDF.from_xml_file(filename) (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) cadena_der_up_down = tree.getChain("base_link", "pie_der_link") cadena_der_down_up = tree.getChain("pie_der_link", "base_link") cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link") cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link") print cadena_der_up_down.getNrOfSegments() fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive( cadena_der_up_down) fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive( cadena_der_down_up) fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive( cadena_izq_up_down) fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive( cadena_izq_down_up) vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down) ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down, fksolver_der_up_down, vik_der_up_down) vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up) ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up, fksolver_der_down_up, vik_der_down_up) vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down) ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down, fksolver_izq_up_down, vik_izq_up_down) vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up) ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up, fksolver_izq_down_up, vik_izq_down_up) nj_izq = cadena_der_up_down.getNrOfJoints() posicionInicial_der_up_down = PyKDL.JntArray(nj_izq) posicionInicial_der_up_down[0] = 0.3 posicionInicial_der_up_down[1] = -0.3 posicionInicial_der_up_down[2] = 0 posicionInicial_der_up_down[3] = 0.6 posicionInicial_der_up_down[4] = -0.3 posicionInicial_der_up_down[5] = -0.3 nj_izq = cadena_izq_up_down.getNrOfJoints() posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq) posicionInicial_izq_up_down[0] = 0.3 posicionInicial_izq_up_down[1] = -0.3 posicionInicial_izq_up_down[2] = 0 posicionInicial_izq_up_down[3] = 0.6 posicionInicial_izq_up_down[4] = -0.3 posicionInicial_izq_up_down[5] = -0.3 nj_izq = cadena_der_down_up.getNrOfJoints() posicionInicial_der_down_up = PyKDL.JntArray(nj_izq) posicionInicial_der_down_up[5] = 0.3 posicionInicial_der_down_up[4] = -0.3 posicionInicial_der_down_up[3] = 0 posicionInicial_der_down_up[2] = 0.6 posicionInicial_der_down_up[1] = -0.3 posicionInicial_der_down_up[0] = -0.3 nj_izq = cadena_izq_down_up.getNrOfJoints() posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq) posicionInicial_izq_down_up[5] = 0.3 posicionInicial_izq_down_up[4] = -0.3 posicionInicial_izq_down_up[3] = 0 posicionInicial_izq_down_up[2] = 0.6 posicionInicial_izq_down_up[1] = -0.3 posicionInicial_izq_down_up[0] = -0.3 print "Forward kinematics" finalFrame_izq_up_down = PyKDL.Frame() finalFrame_izq_down_up = PyKDL.Frame() finalFrame_der_up_down = PyKDL.Frame() finalFrame_der_down_up = PyKDL.Frame() fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down) print "Rotational Matrix of the final Frame: " print finalFrame_izq_up_down.M print "End-effector position: ", finalFrame_izq_up_down.p rospy.init_node('trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set to False to wait for arm to finish before moving head sync = rospy.get_param('~sync', True) # Which joints define the arm? piernas_joints = [ 'cilinder_blue1_box1_der_joint', 'cilinder_blue_box1_der_joint', 'cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint', 'cilinder_blue1_box1_izq_joint', 'cilinder_blue_box1_izq_joint', 'cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint' ] 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 = [0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] print "Inverse Kinematics" q_init = posicionInicial_izq_up_down # initial angles #desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame = finalFrame_izq_up_down desiredFrame.p[0] = finalFrame_izq_up_down.p[0] desiredFrame.p[1] = finalFrame_izq_up_down.p[1] desiredFrame.p[2] = finalFrame_izq_up_down.p[2] print "Desired Position: ", desiredFrame.p q_out = PyKDL.JntArray(6) ik_izq_up_down.CartToJnt(q_init, desiredFrame, q_out) print "Output angles in rads: ", q_out #arm_goal[0] = q_out[0] #arm_goal[1] = q_out[1] #arm_goal[2] = q_out[2] #arm_goal[3] = q_out[3] #arm_goal[4] = q_out[4] #arm_goal[5] = q_out[5] # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') arm_client = actionlib.SimpleActionClient( '/piernas/piernas_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 = piernas_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(2.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) if not sync: # Wait for up to 5 seconds for the motion to complete arm_client.wait_for_result(rospy.Duration(5.0)) rospy.loginfo('...done')
rospy.loginfo("Waiting for gripper_controller...") gripper_client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) gripper_client.wait_for_server() rospy.loginfo("...connected.") 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(5.0) head_goal = FollowJointTrajectoryGoal() head_goal.trajectory = trajectory head_goal.goal_time_tolerance = rospy.Duration(0.0) trajectory = JointTrajectory() trajectory.joint_names = arm_joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = [0.0] * len(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(1.0) trajectory.points.append(JointTrajectoryPoint()) trajectory.points[1].positions = arm_intermediate_positions trajectory.points[1].velocities = [0.0] * len(arm_joint_positions) trajectory.points[1].accelerations = [0.0] * len(arm_joint_positions) trajectory.points[1].time_from_start = rospy.Duration(4.0) trajectory.points.append(JointTrajectoryPoint())
def __init__(self): pkl_file = open('datos_articulaciones.pkl', 'rb') # data1 = pickle.load(pkl_file) # pprint.pprint(data1) # data2 = pickle.load(pkl_file) # pprint.pprint(data2) # print data1['b'][0] # for i in data2: # print i datos = [] try: while True: # loop indefinitely print "LEYENDO" #print pickle.load(pkl_file) datos.append(pickle.load( pkl_file)) # add each item from the file to a list except EOFError: # the exception is used to break the loop pass # we don't need to do anything special pkl_file.close() print len(datos) # print datos_trans for i in datos: pass # print i filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado4.urdf' robot = urdf_parser_py.urdf.URDF.from_xml_file(filename) (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) cadena_der_up_down = tree.getChain("base_link", "pie_der_link") cadena_der_down_up = tree.getChain("pie_der_link", "base_link") cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link") cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link") print cadena_der_up_down.getNrOfSegments() fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive( cadena_der_up_down) fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive( cadena_der_down_up) fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive( cadena_izq_up_down) fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive( cadena_izq_down_up) vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down) ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down, fksolver_der_up_down, vik_der_up_down) vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up) ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up, fksolver_der_down_up, vik_der_down_up) vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down) ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down, fksolver_izq_up_down, vik_izq_up_down) vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up) ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up, fksolver_izq_down_up, vik_izq_down_up) nj_izq = cadena_der_up_down.getNrOfJoints() posicionInicial_der_up_down = PyKDL.JntArray(nj_izq) posicionInicial_der_up_down[0] = 0.3 posicionInicial_der_up_down[1] = -0.3 posicionInicial_der_up_down[2] = 0 posicionInicial_der_up_down[3] = 0.6 posicionInicial_der_up_down[4] = -0.3 posicionInicial_der_up_down[5] = -0.3 nj_izq = cadena_izq_up_down.getNrOfJoints() posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq) posicionInicial_izq_up_down[0] = 0.3 posicionInicial_izq_up_down[1] = -0.3 posicionInicial_izq_up_down[2] = 0.0 posicionInicial_izq_up_down[3] = 0.6 posicionInicial_izq_up_down[4] = -0.3 posicionInicial_izq_up_down[5] = -0.3 nj_izq = cadena_der_down_up.getNrOfJoints() posicionInicial_der_down_up = PyKDL.JntArray(nj_izq) posicionInicial_der_down_up[5] = 0.3 posicionInicial_der_down_up[4] = -0.3 posicionInicial_der_down_up[3] = 0.0 posicionInicial_der_down_up[2] = 0.6 posicionInicial_der_down_up[1] = -0.3 posicionInicial_der_down_up[0] = -0.3 nj_izq = cadena_izq_down_up.getNrOfJoints() posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq) posicionInicial_izq_down_up[5] = 0.3 posicionInicial_izq_down_up[4] = -0.3 posicionInicial_izq_down_up[3] = 0.0 posicionInicial_izq_down_up[2] = 0.6 posicionInicial_izq_down_up[1] = -0.3 posicionInicial_izq_down_up[0] = -0.3 print "Forward kinematics" finalFrame_izq_up_down = PyKDL.Frame() finalFrame_izq_down_up = PyKDL.Frame() finalFrame_der_up_down = PyKDL.Frame() finalFrame_der_down_up = PyKDL.Frame() print "Rotational Matrix of the final Frame: " print finalFrame_izq_up_down.M print "End-effector position: ", finalFrame_izq_up_down.p rospy.init_node('trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set to False to wait for arm to finish before moving head sync = rospy.get_param('~sync', True) # Which joints define the arm? piernas_joints = [ 'cilinder_blue1_box1_der_joint', 'cilinder_blue_box1_der_joint', 'cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint', 'cilinder_blue1_box1_izq_joint', 'cilinder_blue_box1_izq_joint', 'cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint' ] piernas_goal = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] #11111111111111111111111111111111111111111111 print "Inverse Kinematics" fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down) fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up) q_init_izq_up_down = posicionInicial_izq_up_down # initial angles desiredFrame = finalFrame_izq_up_down desiredFrame.p[0] = finalFrame_izq_up_down.p[0] desiredFrame.p[1] = finalFrame_izq_up_down.p[1] desiredFrame.p[2] = finalFrame_izq_up_down.p[2] print "Desired Position: ", desiredFrame.p q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down) print "Output angles in rads: ", q_out_izq_up_down piernas_goal[6] = q_out_izq_up_down[0] piernas_goal[7] = q_out_izq_up_down[1] piernas_goal[8] = q_out_izq_up_down[2] piernas_goal[9] = q_out_izq_up_down[3] piernas_goal[10] = q_out_izq_up_down[4] piernas_goal[11] = q_out_izq_up_down[5] print "Inverse Kinematics" fksolver_der_up_down.JntToCart(posicionInicial_der_up_down, finalFrame_der_up_down) fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up) q_init_der_up_down = posicionInicial_der_up_down # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame = finalFrame_der_up_down desiredFrame.p[0] = finalFrame_der_up_down.p[0] desiredFrame.p[1] = finalFrame_der_up_down.p[1] desiredFrame.p[2] = finalFrame_der_up_down.p[2] + 0.02 #0.02 print "Desired Position: ", desiredFrame.p q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame, q_out_der_up_down) print "Output angles in rads: ", q_out_der_up_down piernas_goal[0] = q_out_der_up_down[0] piernas_goal[1] = q_out_der_up_down[1] piernas_goal[2] = q_out_der_up_down[2] piernas_goal[3] = q_out_der_up_down[3] piernas_goal[4] = q_out_der_up_down[4] piernas_goal[5] = q_out_der_up_down[5] #121212122121212121212121212121212121212121212 piernas_goal12 = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] print "Inverse Kinematics" desiredFrame = PyKDL.Frame() fksolver_izq_up_down.JntToCart(q_out_izq_up_down, desiredFrame) fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up) q_init_izq_up_down = posicionInicial_izq_up_down # initial angles desiredFrame.p[0] = desiredFrame.p[0] desiredFrame.p[1] = desiredFrame.p[1] desiredFrame.p[2] = desiredFrame.p[2] print "Desired Position: ", desiredFrame.p q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down) print "Output angles in rads: ", q_out_izq_up_down piernas_goal12[6] = q_out_izq_up_down[0] piernas_goal12[7] = q_out_izq_up_down[1] piernas_goal12[8] = q_out_izq_up_down[2] piernas_goal12[9] = q_out_izq_up_down[3] piernas_goal12[10] = q_out_izq_up_down[4] piernas_goal12[11] = q_out_izq_up_down[5] print "Inverse Kinematics" desiredFrame0 = PyKDL.Frame() fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame0) fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up) q_init_der_up_down = posicionInicial_der_up_down # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame0.p[0] = desiredFrame0.p[0] desiredFrame0.p[1] = desiredFrame0.p[1] - 0.07 desiredFrame0.p[2] = desiredFrame0.p[2] print "Desired Position: ", desiredFrame0.p q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame0, q_out_der_up_down) print "Output angles in rads: ", q_out_der_up_down piernas_goal12[0] = q_out_der_up_down[0] piernas_goal12[1] = q_out_der_up_down[1] piernas_goal12[2] = q_out_der_up_down[2] piernas_goal12[3] = q_out_der_up_down[3] piernas_goal12[4] = q_out_der_up_down[4] piernas_goal12[5] = q_out_der_up_down[5] # 222222222222222222222222222222222222222222 piernas_goal2 = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] print "Inverse Kinematics" desiredFrame2 = PyKDL.Frame() fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, desiredFrame2) q_init_izq_down_up = posicionInicial_izq_down_up # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame2.p[0] = desiredFrame2.p[0] - 0.06 #0.05 desiredFrame2.p[1] = desiredFrame2.p[1] - 0.06 #0.06 desiredFrame2.p[2] = desiredFrame2.p[2] - 0.01 #0.02 print "Desired Position2222aaa: ", desiredFrame2.p #print desiredFrame2.M q_out_izq_down_up = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_down_up.CartToJnt(q_init_izq_down_up, desiredFrame2, q_out_izq_down_up) print "Output angles in rads2222: ", q_out_izq_down_up piernas_goal2[6] = q_out_izq_down_up[5] #+0.1 piernas_goal2[7] = q_out_izq_down_up[4] #-0.05 piernas_goal2[8] = q_out_izq_down_up[3] piernas_goal2[9] = q_out_izq_down_up[2] piernas_goal2[10] = q_out_izq_down_up[1] piernas_goal2[11] = q_out_izq_down_up[0] #q_out_izq_down_up[4] -=0.1 print "Inverse Kinematics" q_init_der_down_up = PyKDL.JntArray(6) q_init_der_down_up[0] = q_out_der_up_down[ 5] # PROBLEMAS CON LA ASIGNACION q_init_der_down_up[1] = q_out_der_up_down[4] q_init_der_down_up[2] = q_out_der_up_down[3] q_init_der_down_up[3] = q_out_der_up_down[2] q_init_der_down_up[4] = q_out_der_up_down[1] q_init_der_down_up[5] = q_out_der_up_down[0] + 0.08 fksolver_der_down_up.JntToCart(q_init_der_down_up, finalFrame_der_down_up) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame3 = finalFrame_der_down_up desiredFrame3.p[0] = finalFrame_der_down_up.p[0] - 0.05 desiredFrame3.p[1] = finalFrame_der_down_up.p[1] - 0.04 desiredFrame3.p[2] = finalFrame_der_down_up.p[2] - 0.02 print "Desired Position2222der: ", desiredFrame3.p q_out_der_down_up = PyKDL.JntArray(cadena_der_down_up.getNrOfJoints()) ik_der_down_up.CartToJnt(q_init_der_down_up, desiredFrame3, q_out_der_down_up) print "Output angles in rads22222der: ", q_out_der_down_up print "VALOR", q_out_der_up_down[5] piernas_goal2[0] = -0.3 piernas_goal2[1] = -0.3 piernas_goal2[2] = 0 piernas_goal2[3] = 0.6 piernas_goal2[4] = 0.3 piernas_goal2[5] = -0.3 # 333333333333333333333333333333333333333333333333 piernas_goal3 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] print "Inverse Kinematics" desiredFrame4 = PyKDL.Frame() fksolver_izq_down_up.JntToCart(q_out_izq_down_up, desiredFrame4) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame4.p[0] = desiredFrame4.p[0] desiredFrame4.p[1] = desiredFrame4.p[1] - 0.02 desiredFrame4.p[2] = desiredFrame4.p[2] ik_izq_down_up.CartToJnt(q_out_izq_down_up, desiredFrame4, q_out_izq_down_up) q_init_izq_up_down[0] = q_out_izq_down_up[5] q_init_izq_up_down[1] = q_out_izq_down_up[4] q_init_izq_up_down[2] = q_out_izq_down_up[3] q_init_izq_up_down[3] = q_out_izq_down_up[2] q_init_izq_up_down[4] = q_out_izq_down_up[1] q_init_izq_up_down[5] = q_out_izq_down_up[0] desiredFrame5 = PyKDL.Frame() fksolver_izq_up_down.JntToCart(q_init_izq_up_down, desiredFrame5) desiredFrame5.p[0] = desiredFrame5.p[0] desiredFrame5.p[1] = desiredFrame5.p[1] - 0.1 desiredFrame5.p[2] = desiredFrame5.p[2] + 0.01 print "Desired Position: ", desiredFrame5.p q_out_izq_up_down2 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame5, q_out_izq_up_down2) print "Output angles in rads: ", q_out_izq_up_down2 piernas_goal3[6] = q_out_izq_up_down2[0] piernas_goal3[7] = q_out_izq_up_down2[1] piernas_goal3[8] = q_out_izq_up_down2[2] piernas_goal3[9] = q_out_izq_up_down2[3] piernas_goal3[10] = q_out_izq_up_down2[4] #+0.15 piernas_goal3[11] = q_out_izq_up_down2[5] - 0.08 print "Inverse Kinematics" piernas_goal3[0] = -0.3 piernas_goal3[1] = -0.3 piernas_goal3[2] = 0 piernas_goal3[3] = 0.6 piernas_goal3[4] = 0.3 piernas_goal3[5] = -0.3 # 121212122121212121212121212121212121212121212 piernas_goal121 = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] print "Inverse Kinematics" desiredFrame6 = PyKDL.Frame() fksolver_izq_up_down.JntToCart(q_out_izq_up_down2, desiredFrame6) fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up) q_init_izq_up_down = q_out_izq_up_down2 # initial angles #CUIDADO desiredFrame6.p[0] = desiredFrame6.p[0] desiredFrame6.p[1] = desiredFrame6.p[1] - 0.05 desiredFrame6.p[2] = desiredFrame6.p[2] #+0.01 print "Desired Position: ", desiredFrame6.p q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame6, q_out_izq_up_down) print "Output angles in rads_izq_121: ", q_out_izq_up_down piernas_goal121[6] = q_out_izq_up_down[0] piernas_goal121[7] = q_out_izq_up_down[1] piernas_goal121[8] = q_out_izq_up_down[2] piernas_goal121[9] = q_out_izq_up_down[3] piernas_goal121[10] = q_out_izq_up_down[4] piernas_goal121[11] = q_out_izq_up_down[5] print "Inverse Kinematics" desiredFrame06 = PyKDL.Frame() fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame06) fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up) q_init_der_up_down = q_out_der_up_down # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame06.p[0] = desiredFrame06.p[0] desiredFrame06.p[1] = desiredFrame06.p[1] desiredFrame06.p[2] = desiredFrame06.p[2] print "Desired Position: ", desiredFrame06.p q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame06, q_out_der_up_down) print "Output angles in rads: ", q_out_der_up_down q_out_der_up_down21 = PyKDL.JntArray(6) q_out_der_up_down21 = [-.3, -.3, 0, .6, .3, -.3] piernas_goal121[0] = q_out_der_up_down21[0] piernas_goal121[1] = q_out_der_up_down21[1] piernas_goal121[2] = q_out_der_up_down21[2] piernas_goal121[3] = q_out_der_up_down21[3] piernas_goal121[4] = q_out_der_up_down21[4] piernas_goal121[5] = q_out_der_up_down21[5] # 55555555555555555555555555555555555555555 piernas_goal25 = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] print "Inverse Kinematics" desiredFrame7 = PyKDL.Frame() q_init_izq_down_up3 = PyKDL.JntArray(6) q_init_izq_down_up3[0] = q_out_izq_up_down[5] * 1 q_init_izq_down_up3[1] = q_out_izq_up_down[4] * 1 q_init_izq_down_up3[2] = q_out_izq_up_down[3] * 1 q_init_izq_down_up3[3] = q_out_izq_up_down[2] * 1 q_init_izq_down_up3[4] = q_out_izq_up_down[1] * 1 q_init_izq_down_up3[5] = q_out_izq_up_down[0] * 1 fksolver_izq_down_up.JntToCart(q_init_izq_down_up3, desiredFrame7) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame7.p[ 0] = desiredFrame7.p[0] + 0.05 #0.03 # PROBAR A PONERLO MAYOR desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06 #0.04 desiredFrame7.p[2] = desiredFrame7.p[2] + 0.005 print "Desired Position2222: ", desiredFrame7.p q_out_izq_down_up5 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_down_up.CartToJnt(q_init_izq_down_up3, desiredFrame7, q_out_izq_down_up5) print "Output angles in rads2222AAAAA: ", q_out_izq_down_up5 piernas_goal25[6] = q_out_izq_down_up5[5] piernas_goal25[7] = q_out_izq_down_up5[4] piernas_goal25[8] = q_out_izq_down_up5[3] piernas_goal25[9] = q_out_izq_down_up5[2] piernas_goal25[10] = q_out_izq_down_up5[1] - 0.05 piernas_goal25[11] = q_out_izq_down_up5[0] + 0.05 print "Inverse Kinematics" q_init_der_down_up31 = PyKDL.JntArray(6) q_init_der_down_up31[ 0] = q_out_der_up_down21[5] * 1 # PROBLEMAS CON LA ASIGNACION q_init_der_down_up31[1] = q_out_der_up_down21[4] * 1 q_init_der_down_up31[2] = q_out_der_up_down21[3] * 1 q_init_der_down_up31[3] = q_out_der_up_down21[2] * 1 q_init_der_down_up31[4] = q_out_der_up_down21[1] * 1 q_init_der_down_up31[5] = q_out_der_up_down21[0] * 1 desiredFrame7 = PyKDL.Frame() fksolver_der_down_up.JntToCart(q_init_der_down_up31, desiredFrame7) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame7.p[0] = desiredFrame7.p[0] + 0.05 desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06 desiredFrame7.p[2] = desiredFrame7.p[2] - 0.02 print "Desired Position2222der: ", desiredFrame7.p q_out_der_down_up71 = PyKDL.JntArray( cadena_der_down_up.getNrOfJoints()) ik_der_down_up.CartToJnt(q_init_der_down_up31, desiredFrame7, q_out_der_down_up71) print "Output angles in rads22222der: ", q_out_der_down_up71 piernas_goal25[0] = q_out_der_down_up71[5] piernas_goal25[1] = q_out_der_down_up71[4] piernas_goal25[2] = q_out_der_down_up71[3] piernas_goal25[3] = q_out_der_down_up71[2] piernas_goal25[4] = q_out_der_down_up71[1] piernas_goal25[5] = q_out_der_down_up71[0] # 333333333333333333333333333333333333333333333333 piernas_goal341 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] print "Inverse Kinematics" desiredFrame441 = PyKDL.Frame() fksolver_der_down_up.JntToCart(q_out_der_down_up71, desiredFrame441) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame441.p[0] = desiredFrame441.p[0] desiredFrame441.p[1] = desiredFrame441.p[1] - 0.02 desiredFrame441.p[2] = desiredFrame441.p[2] - 0.015 ik_der_down_up.CartToJnt(q_out_der_down_up71, desiredFrame441, q_out_der_down_up71) q_init_der_up_down[0] = q_out_der_down_up71[5] q_init_der_up_down[1] = q_out_der_down_up71[4] q_init_der_up_down[2] = q_out_der_down_up71[3] q_init_der_up_down[3] = q_out_der_down_up71[2] q_init_der_up_down[4] = q_out_der_down_up71[1] q_init_der_up_down[5] = q_out_der_down_up71[0] desiredFrame541 = PyKDL.Frame() fksolver_der_up_down.JntToCart(q_init_der_up_down, desiredFrame541) desiredFrame541.p[0] = desiredFrame541.p[0] - 0.03 desiredFrame541.p[1] = desiredFrame541.p[1] - 0.1 desiredFrame541.p[2] = desiredFrame541.p[2] - 0.01 #nada print "Desired Position: ", desiredFrame541.p q_out_der_up_down241 = PyKDL.JntArray( cadena_izq_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame541, q_out_der_up_down241) # con desiredFrame5 va print "Output angles in radsaaaaa: ", q_out_der_up_down241 print "Inverse Kinematics" piernas_goal341[6] = 0.3 piernas_goal341[7] = -0.3 piernas_goal341[8] = 0 piernas_goal341[9] = 0.6 piernas_goal341[10] = -0.3 piernas_goal341[11] = -0.3 piernas_goal341[0] = q_out_der_up_down241[0] #+0.1 piernas_goal341[1] = q_out_der_up_down241[1] piernas_goal341[2] = q_out_der_up_down241[2] piernas_goal341[3] = q_out_der_up_down241[3] piernas_goal341[4] = q_out_der_up_down241[4] #-0.1 piernas_goal341[5] = q_out_der_up_down241[5] #-0.01 # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') arm_client = actionlib.SimpleActionClient( '/piernas/piernas_controller/follow_joint_trajectory', FollowJointTrajectoryAction) #/piernas/piernas_controller/follow_joint_trajectory arm_client.wait_for_server() rospy.loginfo('...connected.') print "Los datos son", len(datos) # Create a single-point arm trajectory with the piernas_goal as the end-point arm_trajectory = JointTrajectory() arm_trajectory.joint_names = piernas_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = datos[0] arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(2.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[1].positions = datos[1] arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[1].time_from_start = rospy.Duration(4.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[2].positions = datos[2] arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[2].time_from_start = rospy.Duration(6.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[3].positions = datos[3] arm_trajectory.points[3].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[3].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[3].time_from_start = rospy.Duration(8.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[4].positions = datos[4] arm_trajectory.points[4].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[4].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[4].time_from_start = rospy.Duration(10.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[5].positions = datos[5] arm_trajectory.points[5].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[5].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[5].time_from_start = rospy.Duration(12.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[6].positions = datos[6] arm_trajectory.points[6].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[6].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[6].time_from_start = rospy.Duration(14.0) '''arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[7].positions = piernas_goal12 arm_trajectory.points[7].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[7].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[7].time_from_start = rospy.Duration(17.5) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[8].positions = piernas_goal2 arm_trajectory.points[8].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[8].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[8].time_from_start = rospy.Duration(19.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[9].positions = piernas_goal3 arm_trajectory.points[9].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[9].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[9].time_from_start = rospy.Duration(21.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[10].positions = piernas_goal121 arm_trajectory.points[10].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[10].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[10].time_from_start = rospy.Duration(23.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[11].positions = piernas_goal25 arm_trajectory.points[11].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[11].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[11].time_from_start = rospy.Duration(25.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[12].positions = piernas_goal341 arm_trajectory.points[12].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[12].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[12].time_from_start = rospy.Duration(28.0)''' # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') # Create an empty trajectory goal piernas_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above piernas_goal.trajectory = arm_trajectory # Specify zero tolerance for the execution time piernas_goal.goal_time_tolerance = rospy.Duration(0.01) # Send the goal to the action server arm_client.send_goal(piernas_goal) if not sync: # Wait for up to 5 seconds for the motion to complete arm_client.wait_for_result(rospy.Duration(5.0)) arm_client.wait_for_result() print arm_client.get_result() rospy.loginfo('...done')
rospy.loginfo("Waiting for gripper_controller...") gripper_client = actionlib.SimpleActionClient( "gripper_controller/gripper_action", GripperCommandAction) gripper_client.wait_for_server() rospy.loginfo("...connected.") 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(5.0) head_goal = FollowJointTrajectoryGoal() head_goal.trajectory = trajectory head_goal.goal_time_tolerance = rospy.Duration(0.0) trajectory = JointTrajectory() trajectory.joint_names = torso_joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = torso_joint_positions trajectory.points[0].velocities = [0.0] * len(torso_joint_positions) trajectory.points[0].accelerations = [0.0] * len(torso_joint_positions) trajectory.points[0].time_from_start = rospy.Duration(5.0) torso_goal = FollowJointTrajectoryGoal() torso_goal.trajectory = trajectory torso_goal.goal_time_tolerance = rospy.Duration(0.0) trajectory = JointTrajectory()
import actionlib import math from control_msgs.msg import FollowJointTrajectoryAction client = actionlib.SimpleActionClient('/panda_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) client.wait_for_server() from control_msgs.msg import FollowJointTrajectoryGoal goal = FollowJointTrajectoryGoal() from trajectory_msgs.msg import JointTrajectory joint_traj = JointTrajectory() joint_traj.joint_names = ["panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7"] from trajectory_msgs.msg import JointTrajectoryPoint point1 = JointTrajectoryPoint() point2 = JointTrajectoryPoint() point1.positions = [0.33877080806309046, -0.3623406480725775, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795, 0.06015034910227879] point1.velocities = [0., 0., 0., 0., 0., 0., 0.] #point2.positions = [0.33877080806309046, -0.3623406480725775, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795 - math.pi/2, 0.06015034910227879] point2.positions = [0.33877080806309046, -1.5, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795 - math.pi/2, 0.06015034910227879] point2.velocities = [0., 0., 0., 0., 0., 0., 0.] point1.time_from_start = rospy.Duration(2.) point2.time_from_start = rospy.Duration(4.) from copy import deepcopy joint_traj.points = [point1, point2, deepcopy(point1), deepcopy(point2)] joint_traj.points[2].time_from_start += rospy.Duration(4.) joint_traj.points[3].time_from_start += rospy.Duration(4.) goal.trajectory = joint_traj joint_traj.header.stamp = rospy.Time.now()+rospy.Duration(1.0) client.send_goal_and_wait(goal)
def sendWaypointTrajectory(self, waypoints, durations=0., vels=0., accs=0., effs=0.): if not self.ang_cmd_wait(waypoints[0]): print 'Cannot go to the first point in the trajectory' return None # else: # print 'Went to first' if not self.traj_connection: print 'Action server connection was not established' return None joint_traj = JointTrajectory() joint_traj.joint_names = self.arm_joint_names if not durations == 0: if not len(durations) == waypoints: raise Exception( 'The number of duration points is not equal to the number of provided waypoints' ) if not vels == 0: if not len(vels) == waypoints: raise Exception( 'The number velocity points is not equal to the number of provided waypoints' ) if not accs == 0: if not len(accs) == waypoints: raise Exception( 'The number acceleration points is not equal to the number of provided waypoints' ) if not effs == 0: if not len(effs) == waypoints: raise Exception( 'The number effort points is not equal to the number of provided waypoints' ) if not effs == 0: if not (vels == 0 and accs == 0): raise Exception( 'Cannot specify efforts with velocities and accelerations at the same time' ) if (not accs == 0) and vels == 0: raise Exception('Cannot specify accelerations without velocities') total_time_from_start = 0.5 for t in range(0, len(waypoints)): point = JointTrajectoryPoint() waypoint = waypoints[t] if not len(waypoint) == len(joint_traj.joint_names): raise Exception( 'The number of provided joint positions is not equal to the number of available joints for index: ' + str(t)) point.positions = waypoint if not vels == 0.: velocity = vels[t] if not len(velocity) == len(joint_traj.joint_names): raise Exception( 'The number of provided joint velocities is not equal to the number of available joints for index: ' + str(t)) point.velocities = velocity if not accs == 0.: acceleration = accs[t] if not len(acceleration) == len(joint_traj.joint_names): raise Exception( 'The number of provided joint accelerations is not equal to the number of available joints for index: ' + str(t)) point.accelerations = accelerations if not effs == 0.: effort = effs[t] if not len(effort) == len(joint_traj.joint_names): raise Exception( 'The number of provided joint efforts is not equal to the number of available joints for index: ' + str(t)) point.effort = effort if not durations == 0.: point.duration = duration # Deal with increasing time for each trajectory point point.time_from_start = rospy.Duration(total_time_from_start) total_time_from_start = total_time_from_start + 1.0 # Set the points joint_traj.points.append(point) traj_goal = FollowJointTrajectoryGoal() traj_goal.trajectory = joint_traj self.smooth_joint_trajectory_client.send_goal(traj_goal) self.smooth_joint_trajectory_client.wait_for_result() return self.smooth_joint_trajectory_client.get_result()
def push_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name current_state = rospy.wait_for_message('l_arm_controller/state', FollowJointTrajectoryFeedback) start_angles = current_state.actual.positions full_state = rospy.wait_for_message('/joint_states', JointState) req = GetPositionFKRequest() req.header.frame_id = 'base_link' req.fk_link_names = [GRIPPER_LINK_FRAME] req.robot_state.joint_state = full_state if not self.set_planning_scene_diff_client(): rospy.logerr('%s: failed to set planning scene diff' % ACTION_NAME) self.action_server.set_aborted() return pose = self.get_fk_srv(req).pose_stamped[0].pose frame_id = 'base_link' approachpos = [pose.position.x, pose.position.y, pose.position.z] approachquat = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] push_distance = 0.40 grasppos = [ pose.position.x, pose.position.y - push_distance, pose.position.z ] graspquat = [0.00000, 0.00000, 0.70711, -0.70711] # attach object to gripper, they will move together obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.02 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [ collision_operation1, collision_operation2 ] res = self.check_cartesian_path_lists( approachpos, approachquat, grasppos, graspquat, start_angles, frame=frame_id, ordered_collision_operations=ordered_collision_operations) error_code_dict = { ArmNavigationErrorCodes.SUCCESS: 0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1, ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3, ArmNavigationErrorCodes.PLANNING_FAILED: 4 } trajectory_len = len(res.trajectory.joint_trajectory.points) trajectory = [ res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len) ] vels = [ res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len) ] times = [ res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len) ] error_codes = [ error_code_dict[error_code.val] for error_code in res.trajectory_error_codes ] # if even one code is not 0, abort if sum(error_codes) != 0: for ind in range(len(trajectory)): rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) rospy.loginfo("") for ind in range(len(trajectory)): rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind])) rospy.logerr('%s: attempted push failed' % ACTION_NAME) self.action_server.set_aborted() return req = FilterJointTrajectoryRequest() req.trajectory = res.trajectory.joint_trajectory req.trajectory.points = req.trajectory.points[ 1:] # skip zero velocity point req.allowed_time = rospy.Duration(2.0) filt_res = self.trajectory_filter_srv(req) goal = FollowJointTrajectoryGoal() goal.trajectory = filt_res.trajectory goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id) rospy.sleep(0.5) self.trajectory_controller.send_goal(goal) self.trajectory_controller.wait_for_result() state = self.trajectory_controller.get_state() if state == GoalStatus.SUCCEEDED: rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded( PushObjectResult(sound_msg.recorded_sound)) return rospy.logerr('%s: attempted push failed' % ACTION_NAME) self.stop_audio_recording_srv(False) self.action_server.set_aborted()
def __init__(self): pkl_file = open('datos_articulacionespkl.pkl', 'rb') # data1 = pickle.load(pkl_file) # pprint.pprint(data1) # data2 = pickle.load(pkl_file) # pprint.pprint(data2) # print data1['b'][0] # for i in data2: # print i datos = [] try: while True: # loop indefinitely print "LEYENDO" # print pickle.load(pkl_file) datos.append(pickle.load( pkl_file)) # add each item from the file to a list except EOFError: # the exception is used to break the loop pass # we don't need to do anything special pkl_file.close() print len(datos) # print datos_trans for i in datos: pass # print i rospy.init_node('trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set to False to wait for arm to finish before moving head sync = rospy.get_param('~sync', True) # Which joints define the arm? piernas_joints = [ 'cilinder_blue_box1_der_joint', 'cilinder_blue1_box2_der_joint', 'cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint', 'cilinder_blue_box1_izq_joint', 'cilinder_blue1_box2_izq_joint', 'cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint' ] # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') arm_client = actionlib.SimpleActionClient( '/piernas/piernas_controller/follow_joint_trajectory', FollowJointTrajectoryAction) #/piernas/piernas_controller/follow_joint_trajectory arm_client.wait_for_server() rospy.loginfo('...connected.') # Create a single-point arm trajectory with the piernas_goal as the end-point arm_trajectory = JointTrajectory() arm_trajectory.joint_names = piernas_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = datos[0] arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(1.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[1].positions = datos[1] arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[1].time_from_start = rospy.Duration(4.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[2].positions = datos[2] arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[2].time_from_start = rospy.Duration(6.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[3].positions = datos[3] arm_trajectory.points[3].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[3].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[3].time_from_start = rospy.Duration(8.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[4].positions = datos[4] arm_trajectory.points[4].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[4].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[4].time_from_start = rospy.Duration(10.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[5].positions = datos[5] arm_trajectory.points[5].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[5].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[5].time_from_start = rospy.Duration(12.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[6].positions = datos[6] arm_trajectory.points[6].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[6].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[6].time_from_start = rospy.Duration(14.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[7].positions = datos[1] arm_trajectory.points[7].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[7].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[7].time_from_start = rospy.Duration(17.5) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[8].positions = datos[2] arm_trajectory.points[8].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[8].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[8].time_from_start = rospy.Duration(19.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[9].positions = datos[3] arm_trajectory.points[9].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[9].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[9].time_from_start = rospy.Duration(21.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[10].positions = datos[4] arm_trajectory.points[10].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[10].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[10].time_from_start = rospy.Duration(23.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[11].positions = datos[5] arm_trajectory.points[11].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[11].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[11].time_from_start = rospy.Duration(25.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[12].positions = datos[6] arm_trajectory.points[12].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[12].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[12].time_from_start = rospy.Duration(28.0) # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') # Create an empty trajectory goal piernas_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above piernas_goal.trajectory = arm_trajectory # Specify zero tolerance for the execution time piernas_goal.goal_time_tolerance = rospy.Duration(0.01) # Send the goal to the action server arm_client.send_goal(piernas_goal) if not sync: # Wait for up to 5 seconds for the motion to complete arm_client.wait_for_result(rospy.Duration(5.0)) arm_client.wait_for_result() arm_client.send_goal(piernas_goal) print arm_client.get_result() rospy.loginfo('...done')
def __init__(self): filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado4.urdf' robot = urdf_parser_py.urdf.URDF.from_xml_file(filename) (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) cadena_der_up_down = tree.getChain("base_link", "pie_der_link") cadena_der_down_up = tree.getChain("pie_der_link", "base_link") cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link") cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link") print cadena_der_up_down.getNrOfSegments() fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_der_up_down) fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_der_down_up) fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_izq_up_down) fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_izq_down_up) vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down) ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down, fksolver_der_up_down, vik_der_up_down) vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up) ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up, fksolver_der_down_up, vik_der_down_up) vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down) ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down, fksolver_izq_up_down, vik_izq_up_down) vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up) ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up, fksolver_izq_down_up, vik_izq_down_up) nj_izq = cadena_der_up_down.getNrOfJoints() posicionInicial_der_up_down = PyKDL.JntArray(nj_izq) posicionInicial_der_up_down[0] = 0.3 posicionInicial_der_up_down[1] = -0.3 posicionInicial_der_up_down[2] = 0 posicionInicial_der_up_down[3] = 0.6 posicionInicial_der_up_down[4] = -0.3 posicionInicial_der_up_down[5] = -0.3 nj_izq = cadena_izq_up_down.getNrOfJoints() posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq) posicionInicial_izq_up_down[0] = 0.3 posicionInicial_izq_up_down[1] = -0.3 posicionInicial_izq_up_down[2] = 0.0 posicionInicial_izq_up_down[3] = 0.6 posicionInicial_izq_up_down[4] = -0.3 posicionInicial_izq_up_down[5] = -0.3 nj_izq = cadena_der_down_up.getNrOfJoints() posicionInicial_der_down_up = PyKDL.JntArray(nj_izq) posicionInicial_der_down_up[5] = 0.3 posicionInicial_der_down_up[4] = -0.3 posicionInicial_der_down_up[3] = 0.0 posicionInicial_der_down_up[2] = 0.6 posicionInicial_der_down_up[1] = -0.3 posicionInicial_der_down_up[0] = -0.3 nj_izq = cadena_izq_down_up.getNrOfJoints() posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq) posicionInicial_izq_down_up[5] = 0.3 posicionInicial_izq_down_up[4] = -0.3 posicionInicial_izq_down_up[3] = 0.0 posicionInicial_izq_down_up[2] = 0.6 posicionInicial_izq_down_up[1] = -0.3 posicionInicial_izq_down_up[0] = -0.3 print "Forward kinematics" finalFrame_izq_up_down = PyKDL.Frame() finalFrame_izq_down_up = PyKDL.Frame() finalFrame_der_up_down = PyKDL.Frame() finalFrame_der_down_up = PyKDL.Frame() print "Rotational Matrix of the final Frame: " print finalFrame_izq_up_down.M print "End-effector position: ", finalFrame_izq_up_down.p rospy.init_node('trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set to False to wait for arm to finish before moving head sync = rospy.get_param('~sync', True) # Which joints define the arm? piernas_joints = ['cilinder_blue1_box1_der_joint', 'cilinder_blue_box1_der_joint', 'cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint', 'cilinder_blue1_box1_izq_joint', 'cilinder_blue_box1_izq_joint', 'cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint' ] if reset: # Set the arm back to the resting position arm_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] else: # Set a goal configuration for the arm arm_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #11111111111111111111111111111111111111111111 print "Inverse Kinematics" fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down) fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up) q_init_izq_up_down = posicionInicial_izq_up_down # initial angles #desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame = finalFrame_izq_up_down desiredFrame.p[0] = finalFrame_izq_up_down.p[0] desiredFrame.p[1] = finalFrame_izq_up_down.p[1] desiredFrame.p[2] = finalFrame_izq_up_down.p[2] print "Desired Position: ", desiredFrame.p q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down) print "Output angles in rads: ", q_out_izq_up_down arm_goal[6] = q_out_izq_up_down[0] arm_goal[7] = q_out_izq_up_down[1] arm_goal[8] = q_out_izq_up_down[2] arm_goal[9] = q_out_izq_up_down[3] arm_goal[10] = q_out_izq_up_down[4] arm_goal[11] = q_out_izq_up_down[5] print "Inverse Kinematics" fksolver_der_up_down.JntToCart(posicionInicial_der_up_down, finalFrame_der_up_down) fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up) q_init_der_up_down = posicionInicial_der_up_down # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame = finalFrame_der_up_down desiredFrame.p[0] = finalFrame_der_up_down.p[0] desiredFrame.p[1] = finalFrame_der_up_down.p[1]-0.06 desiredFrame.p[2] = finalFrame_der_up_down.p[2]+0.02 print "Desired Position: ", desiredFrame.p q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame, q_out_der_up_down) print "Output angles in rads: ", q_out_der_up_down arm_goal[0] = q_out_der_up_down[0] arm_goal[1] = q_out_der_up_down[1] arm_goal[2] = q_out_der_up_down[2] arm_goal[3] = q_out_der_up_down[3] arm_goal[4] = q_out_der_up_down[4] arm_goal[5] = q_out_der_up_down[5] # 222222222222222222222222222222222222222222 arm_goal2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print "Inverse Kinematics" q_init_izq_down_up = posicionInicial_izq_down_up # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame2 = finalFrame_izq_down_up desiredFrame2.p[0] = finalFrame_izq_down_up.p[0] - 0.05 desiredFrame2.p[1] = finalFrame_izq_down_up.p[1] - 0.06 desiredFrame2.p[2] = finalFrame_izq_down_up.p[2] - 0.02 print "Desired Position: ", desiredFrame2.p q_out_izq_down_up = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_down_up.CartToJnt(q_init_izq_down_up, desiredFrame2, q_out_izq_down_up) print "Output angles in rads: ", q_out_izq_down_up arm_goal2[6] = q_out_izq_down_up[5] arm_goal2[7] = q_out_izq_down_up[4] arm_goal2[8] = q_out_izq_down_up[3] arm_goal2[9] = q_out_izq_down_up[2] arm_goal2[10] = q_out_izq_down_up[1] arm_goal2[11] = q_out_izq_down_up[0] print "Inverse Kinematics" q_init_der_down_up = q_out_der_up_down # initial angles q_init_der_down_up[0] = q_out_der_up_down[5] q_init_der_down_up[1] = q_out_der_up_down[4] q_init_der_down_up[2] = q_out_der_up_down[3] q_init_der_down_up[3] = q_out_der_up_down[2] q_init_der_down_up[4] = q_out_der_up_down[1] q_init_der_down_up[5] = q_out_der_up_down[0] fksolver_der_down_up.JntToCart(q_init_der_down_up,finalFrame_der_down_up) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame3 = finalFrame_der_down_up desiredFrame3.p[0] = finalFrame_der_down_up.p[0] - 0.05 desiredFrame3.p[1] = finalFrame_der_down_up.p[1] - 0.06 desiredFrame3.p[2] = finalFrame_der_down_up.p[2] - 0.02 print "Desired Position: ", desiredFrame3.p q_out_der_down_up = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints()) ik_der_down_up.CartToJnt(q_init_der_down_up, desiredFrame3, q_out_der_down_up) print "Output angles in rads: ", q_out_der_down_up arm_goal2[0] = q_out_der_down_up[5] arm_goal2[1] = q_out_der_down_up[4] arm_goal2[2] = q_out_der_down_up[3] arm_goal2[3] = q_out_der_down_up[2] arm_goal2[4] = q_out_der_down_up[1] arm_goal2[5] = q_out_der_down_up[0] # 333333333333333333333333333333333333333333333333 arm_goal3 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] print "Inverse Kinematics" fksolver_izq_down_up.JntToCart(q_out_izq_down_up,finalFrame_izq_down_up) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame3 = finalFrame_izq_down_up desiredFrame3.p[0] = finalFrame_izq_down_up.p[0] desiredFrame3.p[1] = finalFrame_izq_down_up.p[1] - 0.02 desiredFrame3.p[2] = finalFrame_izq_down_up.p[2] ik_izq_down_up.CartToJnt(q_out_izq_down_up, desiredFrame3, q_out_izq_down_up) q_init_izq_up_down[0] = q_out_izq_down_up[5] q_init_izq_up_down[1] = q_out_izq_down_up[4] q_init_izq_up_down[2] = q_out_izq_down_up[3] q_init_izq_up_down[3] = q_out_izq_down_up[2] q_init_izq_up_down[4] = q_out_izq_down_up[1] q_init_izq_up_down[5] = q_out_izq_down_up[0] fksolver_izq_up_down.JntToCart(q_init_izq_up_down,finalFrame_izq_up_down) desiredFrame4 = finalFrame_izq_up_down desiredFrame4.p[0] = finalFrame_izq_up_down.p[0] desiredFrame4.p[1] = finalFrame_izq_up_down.p[1] - 0.13 desiredFrame4.p[2] = finalFrame_izq_up_down.p[2] + 0.012 print "Desired Position: ", desiredFrame4.p q_out_izq_up_down2 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame4, q_out_izq_up_down2) print "Output angles in rads: ", q_out_izq_up_down2 arm_goal3[6] = q_out_izq_up_down2[0] arm_goal3[7] = q_out_izq_up_down2[1] arm_goal3[8] = q_out_izq_up_down2[2] arm_goal3[9] = q_out_izq_up_down2[3] arm_goal3[10] = q_out_izq_up_down2[4] arm_goal3[11] = q_out_izq_up_down2[5] print "Inverse Kinematics" arm_goal3[0] = -0.4 arm_goal3[1] = -0.25 arm_goal3[2] = 0 arm_goal3[3] = 0.5 arm_goal3[4] = 0.4 arm_goal3[5] = -0.25 # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') arm_client = actionlib.SimpleActionClient('/piernas/piernas_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 = piernas_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(1.0) #arm_trajectory.points.append(JointTrajectoryPoint()) #arm_trajectory.points[1].positions = arm_goal2 #arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints] #arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints] #arm_trajectory.points[1].time_from_start = rospy.Duration(3.0) #arm_trajectory.points.append(JointTrajectoryPoint()) #arm_trajectory.points[2].positions = arm_goal3 #arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints] #arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints] #arm_trajectory.points[2].time_from_start = rospy.Duration(4.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.01) # Send the goal to the action server arm_client.send_goal(arm_goal) if not sync: # Wait for up to 5 seconds for the motion to complete arm_client.wait_for_result(rospy.Duration(5.0)) rospy.loginfo('...done')
def wipe(self, start, finish): dist = int(round( 200 * self.calc_dist(start, finish))) # 1 point per cm of separation print "Steps: %s" % dist x_step = finish.pose.position.x - start.pose.position.x y_step = finish.pose.position.y - start.pose.position.y z_step = finish.pose.position.z - start.pose.position.z #print "Increments: %s,%s,%s" %(x_step, y_step, z_step) qs = [ start.pose.orientation.x, start.pose.orientation.y, start.pose.orientation.z, start.pose.orientation.w ] qf = [ finish.pose.orientation.x, finish.pose.orientation.y, finish.pose.orientation.z, finish.pose.orientation.w ] steps = [] #print "Start: %s" %start #print "Finish: %s" %finish for i in range(dist): frac = float(i) / float(dist) steps.append(PoseStamped()) steps[i].header.stamp = rospy.Time.now() steps[i].header.frame_id = start.header.frame_id steps[i].pose.position.x = start.pose.position.x + x_step * frac steps[i].pose.position.y = start.pose.position.y + y_step * frac steps[i].pose.position.z = start.pose.position.z + z_step * frac new_q = transformations.quaternion_slerp(qs, qf, frac) steps[i].pose.orientation.x = new_q[0] steps[i].pose.orientation.y = new_q[1] steps[i].pose.orientation.z = new_q[2] steps[i].pose.orientation.w = new_q[3] steps.append(finish) #print "Steps:" #print steps #raw_input("Press Enter to continue") rospy.loginfo("Planning straight-line path, please wait") self.wt_log_out.publish( data="Planning straight-line path, please wait") rospy.loginfo("Initiating wipe action") self.blind_move(finish) self.r_arm_traj_client.wait_for_result(rospy.Duration(20)) rospy.loginfo("At beginning of path") pts = [] for i, p in enumerate(steps): frac = float(i) / float(len(steps)) request = self.form_ik_request(p) if not i == 0: request.ik_request.ik_seed_state.joint_state.position = seed ik_goal = self.ik_pose_proxy(request) pts.append(ik_goal.solution.joint_state.position) seed = pts[i] points = [] for i in range(len(pts) - 1): angs1 = pts[i] angs2 = pts[i + 1] increm = np.subtract(angs2, angs1) for j in range(10): points.append(np.add(angs1, np.multiply(0.1 * j, increm))) #points = np.unwrap(points,1) p1 = points traj = JointTrajectory() traj.header.frame_id = steps[0].header.frame_id traj.joint_names = self.ik_info.kinematic_solver_info.joint_names times = [] for i in range(len(points)): frac = float(i) / float(len(points)) traj.points.append(JointTrajectoryPoint()) traj.points[i].positions = points[i] traj.points[i].velocities = [0] * 7 times.append(rospy.Duration(frac * dist * 0.2)) 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) #print "Steps: %s" %steps count = 0 while count < 6: traj_goal.trajectory.points.reverse() for i in range(len(times)): traj_goal.trajectory.points[i].time_from_start = times[i] if count == 0: print traj_goal.trajectory.points raw_input("Review Trajectory Goal") traj_goal.trajectory.header.stamp = rospy.Time.now() self.r_arm_follow_traj_client.send_goal(traj_goal) self.r_arm_follow_traj_client.wait_for_result(rospy.Duration(20)) rospy.sleep(0.5) count += 1 #traj_goal = JointTrajectoryGoal() #traj_goal.trajectory = traj #print "Steps: %s" %steps #count = 0 # #while count < 6: #traj_goal.trajectory.points.reverse() #for i in range(len(times)): #traj_goal.trajectory.points[i].time_from_start = times[i] #print traj_goal #raw_input("Review Trajectory Goal") ##print "Traj goal start:" ##print traj_goal.trajectory.points[0].positions ##print "Traj goal end:" ##print traj_goal.trajectory.points[-1].positions #traj_goal.trajectory.header.stamp = rospy.Time.now() #self.r_arm_traj_client.send_goal(traj_goal) #self.r_arm_traj_client.wait_for_result(rospy.Duration(20)) #rospy.sleep(0.5) #count += 1 rospy.loginfo("Done Wiping") self.wt_log_out.publish(data="Done Wiping") self.linear_move(Float32(-0.15))
randoms = [] max_value = max - min for i in range(n): randoms.append((random.random() * max_value) + min) # this scales the value between min and max return randoms if __name__ == '__main__': rospy.init_node('random_left_leg_goals') ctl_as = SimpleActionClient(CONTROLLER_AS, FollowJointTrajectoryAction) rospy.loginfo("Connecting to " + CONTROLLER_AS) ctl_as.wait_for_server() rospy.loginfo("Connected.") while not rospy.is_shutdown(): fjtg = FollowJointTrajectoryGoal() jt = JointTrajectory() #jt.joint_names = ['leg_left_1_joint', 'leg_left_2_joint', 'leg_left_3_joint', 'leg_left_4_joint', 'leg_left_5_joint', 'leg_left_6_joint'] jt.joint_names = JOINT_NAMES jtp = JointTrajectoryPoint() jtp.positions = get_n_randoms(len(JOINT_NAMES), -4.0, 4.0) jtp.velocities = [0.0] * len(JOINT_NAMES) #jtp.accelerations = [0.0] * len(JOINT_NAMES) #jtp.effort = [0.0] * len(JOINT_NAMES) jtp.time_from_start = rospy.Duration(random.random() + 0.5) # 0.5s to 1.5s jt.points.append(jtp) fjtg.trajectory = jt ctl_as.send_goal_and_wait(fjtg) rospy.loginfo("Sent: " + str(jt)) rospy.sleep(jtp.time_from_start)
def run(self, pan_amount=0.0, tilt_amount=0.0, ignore_errors=True): """ The run function for this step Args: pan_amount (float) : the amount in radians to pan the head by. left (+ve) and right (-ve) tilt_amount (float) : the amount in radians to tilt the head by. down (+ve) and up (-ve) ignore_errors (bool) : if ``True``, ignore the trajectory time limit exceeded errors that sometimes pop up from the :const:`HEAD_ACTION_SERVER` .. seealso:: :meth:`task_executor.abstract_step.AbstractStep.run` """ rospy.loginfo("Action {}: Pan by amount {}, Tilt by amount {}".format( self.name, pan_amount, tilt_amount)) # Update the desired pan and tilt positions based on the joint limits desired_pan = max(LookPanTiltAction.HEAD_PAN_MIN, self._current_pan + pan_amount) desired_pan = min(LookPanTiltAction.HEAD_PAN_MAX, desired_pan) desired_tilt = max(LookPanTiltAction.HEAD_TILT_MIN, self._current_tilt + tilt_amount) desired_tilt = min(LookPanTiltAction.HEAD_TILT_MAX, desired_tilt) # Create and send the goal height trajectory = JointTrajectory() trajectory.joint_names = self._joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = [desired_pan, desired_tilt] trajectory.points[0].velocities = [0.0, 0.0] trajectory.points[0].accelerations = [0.0, 0.0] trajectory.points[0].time_from_start = rospy.Duration(self._duration) follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = trajectory self._head_client.send_goal(follow_goal) self.notify_action_send_goal(LookPanTiltAction.HEAD_ACTION_SERVER, follow_goal) # Yield an empty dict while we're executing while self._head_client.get_state( ) in AbstractStep.RUNNING_GOAL_STATES: yield self.set_running() # Wait for a result and yield based on how we exited status = self._head_client.get_state() self._head_client.wait_for_result() result = self._head_client.get_result() self.notify_action_recv_result(LookPanTiltAction.HEAD_ACTION_SERVER, status, result) if status == GoalStatus.PREEMPTED: yield self.set_preempted(action=self.name, status=status, goal=[desired_pan, desired_tilt], result=result) elif status == GoalStatus.SUCCEEDED or ignore_errors: yield self.set_succeeded() else: yield self.set_aborted(action=self.name, status=status, goal=[desired_pan, desired_tilt], result=result)
def __init__(self): filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado4.urdf' datos_articulaciones = open('datos_articulaciones.pkl', 'wb') robot = urdf_parser_py.urdf.URDF.from_xml_file(filename) (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) cadena_der_up_down = tree.getChain("base_link", "pie_der_link") cadena_der_down_up = tree.getChain("pie_der_link", "base_link") cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link") cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link") print cadena_der_up_down.getNrOfSegments() fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_der_up_down) fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_der_down_up) fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_izq_up_down) fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_izq_down_up) vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down) ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down, fksolver_der_up_down, vik_der_up_down) vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up) ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up, fksolver_der_down_up, vik_der_down_up) vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down) ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down, fksolver_izq_up_down, vik_izq_up_down) vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up) ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up, fksolver_izq_down_up, vik_izq_down_up) nj_izq = cadena_der_up_down.getNrOfJoints() posicionInicial_der_up_down = PyKDL.JntArray(nj_izq) posicionInicial_der_up_down[0] = 0.3 posicionInicial_der_up_down[1] = -0.3 posicionInicial_der_up_down[2] = 0 posicionInicial_der_up_down[3] = 0.6 posicionInicial_der_up_down[4] = -0.3 posicionInicial_der_up_down[5] = -0.3 nj_izq = cadena_izq_up_down.getNrOfJoints() posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq) posicionInicial_izq_up_down[0] = 0.3 posicionInicial_izq_up_down[1] = -0.3 posicionInicial_izq_up_down[2] = 0.0 posicionInicial_izq_up_down[3] = 0.6 posicionInicial_izq_up_down[4] = -0.3 posicionInicial_izq_up_down[5] = -0.3 nj_izq = cadena_der_down_up.getNrOfJoints() posicionInicial_der_down_up = PyKDL.JntArray(nj_izq) posicionInicial_der_down_up[5] = 0.3 posicionInicial_der_down_up[4] = -0.3 posicionInicial_der_down_up[3] = 0.0 posicionInicial_der_down_up[2] = 0.6 posicionInicial_der_down_up[1] = -0.3 posicionInicial_der_down_up[0] = -0.3 nj_izq = cadena_izq_down_up.getNrOfJoints() posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq) posicionInicial_izq_down_up[5] = 0.3 posicionInicial_izq_down_up[4] = -0.3 posicionInicial_izq_down_up[3] = 0.0 posicionInicial_izq_down_up[2] = 0.6 posicionInicial_izq_down_up[1] = -0.3 posicionInicial_izq_down_up[0] = -0.3 print "Forward kinematics" finalFrame_izq_up_down = PyKDL.Frame() finalFrame_izq_down_up = PyKDL.Frame() finalFrame_der_up_down = PyKDL.Frame() finalFrame_der_down_up = PyKDL.Frame() print "Rotational Matrix of the final Frame: " print finalFrame_izq_up_down.M print "End-effector position: ", finalFrame_izq_up_down.p rospy.init_node('trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Set to False to wait for arm to finish before moving head sync = rospy.get_param('~sync', True) # Which joints define the arm? piernas_joints = ['cilinder_blue1_box1_der_joint', 'cilinder_blue_box1_der_joint', 'cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint', 'cilinder_blue1_box1_izq_joint', 'cilinder_blue_box1_izq_joint', 'cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint' ] piernas_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #11111111111111111111111111111111111111111111 print "Inverse Kinematics" fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down) fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up) q_init_izq_up_down = posicionInicial_izq_up_down # initial angles desiredFrame = finalFrame_izq_up_down desiredFrame.p[0] = finalFrame_izq_up_down.p[0] desiredFrame.p[1] = finalFrame_izq_up_down.p[1] desiredFrame.p[2] = finalFrame_izq_up_down.p[2] print "Desired Position: ", desiredFrame.p q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down) print "Output angles in rads: ", q_out_izq_up_down piernas_goal[6] = q_out_izq_up_down[0] piernas_goal[7] = q_out_izq_up_down[1] piernas_goal[8] = q_out_izq_up_down[2] piernas_goal[9] = q_out_izq_up_down[3] piernas_goal[10] = q_out_izq_up_down[4] piernas_goal[11] = q_out_izq_up_down[5] print "Inverse Kinematics" fksolver_der_up_down.JntToCart(posicionInicial_der_up_down, finalFrame_der_up_down) fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up) q_init_der_up_down = posicionInicial_der_up_down # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame = finalFrame_der_up_down desiredFrame.p[0] = finalFrame_der_up_down.p[0] desiredFrame.p[1] = finalFrame_der_up_down.p[1] desiredFrame.p[2] = finalFrame_der_up_down.p[2]+0.02 #0.02 print "Desired Position: ", desiredFrame.p q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame, q_out_der_up_down) print "Output angles in rads: ", q_out_der_up_down piernas_goal[0] = q_out_der_up_down[0] piernas_goal[1] = q_out_der_up_down[1] piernas_goal[2] = q_out_der_up_down[2] piernas_goal[3] = q_out_der_up_down[3] piernas_goal[4] = q_out_der_up_down[4] piernas_goal[5] = q_out_der_up_down[5] #121212122121212121212121212121212121212121212 piernas_goal12 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print "Inverse Kinematics" desiredFrame = PyKDL.Frame() fksolver_izq_up_down.JntToCart(q_out_izq_up_down, desiredFrame) fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up) q_init_izq_up_down = posicionInicial_izq_up_down # initial angles desiredFrame.p[0] = desiredFrame.p[0] desiredFrame.p[1] = desiredFrame.p[1] desiredFrame.p[2] = desiredFrame.p[2] print "Desired Position: ", desiredFrame.p q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down) print "Output angles in rads: ", q_out_izq_up_down piernas_goal12[6] = q_out_izq_up_down[0] piernas_goal12[7] = q_out_izq_up_down[1] piernas_goal12[8] = q_out_izq_up_down[2] piernas_goal12[9] = q_out_izq_up_down[3] piernas_goal12[10] = q_out_izq_up_down[4] piernas_goal12[11] = q_out_izq_up_down[5] print "Inverse Kinematics" desiredFrame0 = PyKDL.Frame() fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame0) fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up) q_init_der_up_down = posicionInicial_der_up_down # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame0.p[0] = desiredFrame0.p[0] desiredFrame0.p[1] = desiredFrame0.p[1] -0.07 desiredFrame0.p[2] = desiredFrame0.p[2] print "Desired Position: ", desiredFrame0.p q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame0, q_out_der_up_down) print "Output angles in rads: ", q_out_der_up_down piernas_goal12[0] = q_out_der_up_down[0] piernas_goal12[1] = q_out_der_up_down[1] piernas_goal12[2] = q_out_der_up_down[2] piernas_goal12[3] = q_out_der_up_down[3] piernas_goal12[4] = q_out_der_up_down[4] piernas_goal12[5] = q_out_der_up_down[5] # 222222222222222222222222222222222222222222 piernas_goal2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print "Inverse Kinematics" desiredFrame2 = PyKDL.Frame() fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, desiredFrame2) q_init_izq_down_up = posicionInicial_izq_down_up # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame2.p[0] = desiredFrame2.p[0] -0.06 #0.05 desiredFrame2.p[1] = desiredFrame2.p[1] -0.06#0.06 desiredFrame2.p[2] = desiredFrame2.p[2] -0.01 #0.02 print "Desired Position2222aaa: ", desiredFrame2.p #print desiredFrame2.M q_out_izq_down_up = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_down_up.CartToJnt(q_init_izq_down_up, desiredFrame2, q_out_izq_down_up) print "Output angles in rads2222: ", q_out_izq_down_up piernas_goal2[6] = q_out_izq_down_up[5]#+0.1 piernas_goal2[7] = q_out_izq_down_up[4]#-0.05 piernas_goal2[8] = q_out_izq_down_up[3] piernas_goal2[9] = q_out_izq_down_up[2] piernas_goal2[10] = q_out_izq_down_up[1] piernas_goal2[11] = q_out_izq_down_up[0] #q_out_izq_down_up[4] -=0.1 print "Inverse Kinematics" q_init_der_down_up = PyKDL.JntArray(6) q_init_der_down_up[0] = q_out_der_up_down[5] # PROBLEMAS CON LA ASIGNACION q_init_der_down_up[1] = q_out_der_up_down[4] q_init_der_down_up[2] = q_out_der_up_down[3] q_init_der_down_up[3] = q_out_der_up_down[2] q_init_der_down_up[4] = q_out_der_up_down[1] q_init_der_down_up[5] = q_out_der_up_down[0]+0.08 fksolver_der_down_up.JntToCart(q_init_der_down_up,finalFrame_der_down_up) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame3 = finalFrame_der_down_up desiredFrame3.p[0] = finalFrame_der_down_up.p[0]- 0.05 desiredFrame3.p[1] = finalFrame_der_down_up.p[1]- 0.04 desiredFrame3.p[2] = finalFrame_der_down_up.p[2]-0.02 print "Desired Position2222der: ", desiredFrame3.p q_out_der_down_up = PyKDL.JntArray(cadena_der_down_up.getNrOfJoints()) ik_der_down_up.CartToJnt(q_init_der_down_up, desiredFrame3, q_out_der_down_up) print "Output angles in rads22222der: ", q_out_der_down_up print "VALOR", q_out_der_up_down[5] piernas_goal2[0] = -0.3 piernas_goal2[1] = -0.3 piernas_goal2[2] = 0 piernas_goal2[3] = 0.6 piernas_goal2[4] = 0.3 piernas_goal2[5] = -0.3 # 333333333333333333333333333333333333333333333333 piernas_goal3 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] print "Inverse Kinematics" desiredFrame4 = PyKDL.Frame() fksolver_izq_down_up.JntToCart(q_out_izq_down_up,desiredFrame4) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame4.p[0] = desiredFrame4.p[0] desiredFrame4.p[1] = desiredFrame4.p[1] - 0.02 desiredFrame4.p[2] = desiredFrame4.p[2] ik_izq_down_up.CartToJnt(q_out_izq_down_up, desiredFrame4, q_out_izq_down_up) q_init_izq_up_down[0] = q_out_izq_down_up[5] q_init_izq_up_down[1] = q_out_izq_down_up[4] q_init_izq_up_down[2] = q_out_izq_down_up[3] q_init_izq_up_down[3] = q_out_izq_down_up[2] q_init_izq_up_down[4] = q_out_izq_down_up[1] q_init_izq_up_down[5] = q_out_izq_down_up[0] desiredFrame5 = PyKDL.Frame() fksolver_izq_up_down.JntToCart(q_init_izq_up_down,desiredFrame5) desiredFrame5.p[0] = desiredFrame5.p[0] desiredFrame5.p[1] = desiredFrame5.p[1] - 0.1 desiredFrame5.p[2] = desiredFrame5.p[2]+0.01 print "Desired Position: ", desiredFrame5.p q_out_izq_up_down2 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame5, q_out_izq_up_down2) print "Output angles in rads: ", q_out_izq_up_down2 piernas_goal3[6] = q_out_izq_up_down2[0] piernas_goal3[7] = q_out_izq_up_down2[1] piernas_goal3[8] = q_out_izq_up_down2[2] piernas_goal3[9] = q_out_izq_up_down2[3] piernas_goal3[10] = q_out_izq_up_down2[4]#+0.15 piernas_goal3[11] = q_out_izq_up_down2[5]-0.08 print "Inverse Kinematics" piernas_goal3[0] = -0.3 piernas_goal3[1] = -0.3 piernas_goal3[2] = 0 piernas_goal3[3] = 0.6 piernas_goal3[4] = 0.3 piernas_goal3[5] = -0.3 # 121212122121212121212121212121212121212121212 piernas_goal121 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print "Inverse Kinematics" desiredFrame6 = PyKDL.Frame() fksolver_izq_up_down.JntToCart(q_out_izq_up_down2, desiredFrame6) fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up) q_init_izq_up_down = q_out_izq_up_down2 # initial angles #CUIDADO desiredFrame6.p[0] = desiredFrame6.p[0] desiredFrame6.p[1] = desiredFrame6.p[1]-0.05 desiredFrame6.p[2] = desiredFrame6.p[2]#+0.01 print "Desired Position: ", desiredFrame6.p q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame6, q_out_izq_up_down) print "Output angles in rads_izq_121: ", q_out_izq_up_down piernas_goal121[6] = q_out_izq_up_down[0] piernas_goal121[7] = q_out_izq_up_down[1] piernas_goal121[8] = q_out_izq_up_down[2] piernas_goal121[9] = q_out_izq_up_down[3] piernas_goal121[10] = q_out_izq_up_down[4] piernas_goal121[11] = q_out_izq_up_down[5] print "Inverse Kinematics" desiredFrame06 = PyKDL.Frame() fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame06) fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up) q_init_der_up_down = q_out_der_up_down # initial angles # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame06.p[0] = desiredFrame06.p[0] desiredFrame06.p[1] = desiredFrame06.p[1] desiredFrame06.p[2] = desiredFrame06.p[2] print "Desired Position: ", desiredFrame06.p q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame06, q_out_der_up_down) print "Output angles in rads: ", q_out_der_up_down q_out_der_up_down21 = PyKDL.JntArray(6) q_out_der_up_down21 = [-.3, -.3, 0, .6, .3, -.3 ] piernas_goal121[0] = q_out_der_up_down21[0] piernas_goal121[1] = q_out_der_up_down21[1] piernas_goal121[2] = q_out_der_up_down21[2] piernas_goal121[3] = q_out_der_up_down21[3] piernas_goal121[4] = q_out_der_up_down21[4] piernas_goal121[5] = q_out_der_up_down21[5] # 55555555555555555555555555555555555555555 piernas_goal25 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print "Inverse Kinematics" desiredFrame7= PyKDL.Frame() q_init_izq_down_up3 = PyKDL.JntArray(6) q_init_izq_down_up3[0] = q_out_izq_up_down[5] * 1 q_init_izq_down_up3[1] = q_out_izq_up_down[4] * 1 q_init_izq_down_up3[2] = q_out_izq_up_down[3] * 1 q_init_izq_down_up3[3] = q_out_izq_up_down[2] * 1 q_init_izq_down_up3[4] = q_out_izq_up_down[1] * 1 q_init_izq_down_up3[5] = q_out_izq_up_down[0] * 1 fksolver_izq_down_up.JntToCart(q_init_izq_down_up3, desiredFrame7) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame7.p[0] = desiredFrame7.p[0] + 0.05#0.03 # PROBAR A PONERLO MAYOR desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06#0.04 desiredFrame7.p[2] = desiredFrame7.p[2] + 0.005 print "Desired Position2222: ", desiredFrame7.p q_out_izq_down_up5 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_izq_down_up.CartToJnt(q_init_izq_down_up3, desiredFrame7, q_out_izq_down_up5) print "Output angles in rads2222AAAAA: ", q_out_izq_down_up5 piernas_goal25[6] = q_out_izq_down_up5[5] piernas_goal25[7] = q_out_izq_down_up5[4] piernas_goal25[8] = q_out_izq_down_up5[3] piernas_goal25[9] = q_out_izq_down_up5[2] piernas_goal25[10] = q_out_izq_down_up5[1]-0.05 piernas_goal25[11] = q_out_izq_down_up5[0]+0.05 print "Inverse Kinematics" q_init_der_down_up31 = PyKDL.JntArray(6) q_init_der_down_up31[0] = q_out_der_up_down21[5] *1 # PROBLEMAS CON LA ASIGNACION q_init_der_down_up31[1] = q_out_der_up_down21[4] *1 q_init_der_down_up31[2] = q_out_der_up_down21[3] *1 q_init_der_down_up31[3] = q_out_der_up_down21[2] *1 q_init_der_down_up31[4] = q_out_der_up_down21[1] *1 q_init_der_down_up31[5] = q_out_der_up_down21[0] *1 desiredFrame7 = PyKDL.Frame() fksolver_der_down_up.JntToCart(q_init_der_down_up31, desiredFrame7) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame7.p[0] = desiredFrame7.p[0] + 0.05 desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06 desiredFrame7.p[2] = desiredFrame7.p[2] - 0.02 print "Desired Position2222der: ", desiredFrame7.p q_out_der_down_up71 = PyKDL.JntArray(cadena_der_down_up.getNrOfJoints()) ik_der_down_up.CartToJnt(q_init_der_down_up31, desiredFrame7, q_out_der_down_up71) print "Output angles in rads22222der: ", q_out_der_down_up71 piernas_goal25[0] = q_out_der_down_up71[5] piernas_goal25[1] = q_out_der_down_up71[4] piernas_goal25[2] = q_out_der_down_up71[3] piernas_goal25[3] = q_out_der_down_up71[2] piernas_goal25[4] = q_out_der_down_up71[1] piernas_goal25[5] = q_out_der_down_up71[0] # 333333333333333333333333333333333333333333333333 piernas_goal341 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] print "Inverse Kinematics" desiredFrame441 = PyKDL.Frame() fksolver_der_down_up.JntToCart(q_out_der_down_up71, desiredFrame441) # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1)) desiredFrame441.p[0] = desiredFrame441.p[0] desiredFrame441.p[1] = desiredFrame441.p[1] - 0.02 desiredFrame441.p[2] = desiredFrame441.p[2] - 0.015 ik_der_down_up.CartToJnt(q_out_der_down_up71, desiredFrame441, q_out_der_down_up71) q_init_der_up_down[0] = q_out_der_down_up71[5] q_init_der_up_down[1] = q_out_der_down_up71[4] q_init_der_up_down[2] = q_out_der_down_up71[3] q_init_der_up_down[3] = q_out_der_down_up71[2] q_init_der_up_down[4] = q_out_der_down_up71[1] q_init_der_up_down[5] = q_out_der_down_up71[0] desiredFrame541 = PyKDL.Frame() fksolver_der_up_down.JntToCart(q_init_der_up_down, desiredFrame541) desiredFrame541.p[0] = desiredFrame541.p[0] - 0.03 desiredFrame541.p[1] = desiredFrame541.p[1] - 0.1 desiredFrame541.p[2] = desiredFrame541.p[2] - 0.01 #nada print "Desired Position: ", desiredFrame541.p q_out_der_up_down241 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints()) ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame541, q_out_der_up_down241)# con desiredFrame5 va print "Output angles in radsaaaaa: ", q_out_der_up_down241 print "Inverse Kinematics" piernas_goal341[6] = 0.3 piernas_goal341[7] = -0.3 piernas_goal341[8] = 0 piernas_goal341[9] = 0.6 piernas_goal341[10] = -0.3 piernas_goal341[11] = -0.3 piernas_goal341[0] = q_out_der_up_down241[0]#+0.1 piernas_goal341[1] = q_out_der_up_down241[1] piernas_goal341[2] = q_out_der_up_down241[2] piernas_goal341[3] = q_out_der_up_down241[3] piernas_goal341[4] = q_out_der_up_down241[4]#-0.1 piernas_goal341[5] = q_out_der_up_down241[5]#-0.01 pickle.dump(piernas_goal, datos_articulaciones, -1) pickle.dump(piernas_goal12, datos_articulaciones, -1) pickle.dump(piernas_goal2, datos_articulaciones, -1) pickle.dump(piernas_goal3, datos_articulaciones, -1) pickle.dump(piernas_goal121, datos_articulaciones, -1) pickle.dump(piernas_goal25, datos_articulaciones, -1) pickle.dump(piernas_goal341, datos_articulaciones, -1) datos_articulaciones.close() # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') arm_client = actionlib.SimpleActionClient('/piernas/piernas_controller/follow_joint_trajectory', FollowJointTrajectoryAction) #/piernas/piernas_controller/follow_joint_trajectory arm_client.wait_for_server() rospy.loginfo('...connected.') # Create a single-point arm trajectory with the piernas_goal as the end-point arm_trajectory = JointTrajectory() arm_trajectory.joint_names = piernas_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = piernas_goal arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(2.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[1].positions = piernas_goal12 arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[1].time_from_start = rospy.Duration(4.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[2].positions = piernas_goal2 arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[2].time_from_start = rospy.Duration(6.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[3].positions = piernas_goal3 arm_trajectory.points[3].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[3].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[3].time_from_start = rospy.Duration(8.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[4].positions = piernas_goal121 arm_trajectory.points[4].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[4].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[4].time_from_start = rospy.Duration(10.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[5].positions = piernas_goal25 arm_trajectory.points[5].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[5].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[5].time_from_start = rospy.Duration(12.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[6].positions = piernas_goal341 arm_trajectory.points[6].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[6].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[6].time_from_start = rospy.Duration(14.0) '''arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[7].positions = piernas_goal12 arm_trajectory.points[7].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[7].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[7].time_from_start = rospy.Duration(17.5) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[8].positions = piernas_goal2 arm_trajectory.points[8].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[8].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[8].time_from_start = rospy.Duration(19.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[9].positions = piernas_goal3 arm_trajectory.points[9].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[9].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[9].time_from_start = rospy.Duration(21.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[10].positions = piernas_goal121 arm_trajectory.points[10].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[10].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[10].time_from_start = rospy.Duration(23.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[11].positions = piernas_goal25 arm_trajectory.points[11].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[11].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[11].time_from_start = rospy.Duration(25.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[12].positions = piernas_goal341 arm_trajectory.points[12].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[12].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[12].time_from_start = rospy.Duration(28.0)''' # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') # Create an empty trajectory goal piernas_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above piernas_goal.trajectory = arm_trajectory # Specify zero tolerance for the execution time piernas_goal.goal_time_tolerance = rospy.Duration(0.01) # Send the goal to the action server arm_client.send_goal(piernas_goal) if not sync: # Wait for up to 5 seconds for the motion to complete arm_client.wait_for_result(rospy.Duration(5.0)) arm_client.wait_for_result() print arm_client.get_result() rospy.loginfo('...done')
mpr.allowed_planning_time = 3.0 # [s] # print mpr try: req = rospy.ServiceProxy('/plan_kinematic_path', GetMotionPlan) res = req(mpr) traj = res.motion_plan_response.trajectory except rospy.ServiceException, e: print "Service call failed: %s" % e client = actionlib.SimpleActionClient(self.joint_traj_action_topic, FollowJointTrajectoryAction) client.wait_for_server() # http://docs.ros.org/diamondback/api/control_msgs/html/index-msg.html goal = FollowJointTrajectoryGoal() goal.trajectory = traj.joint_trajectory client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(15.0)) # http://docs.ros.org/api/actionlib/html/classactionlib_1_1simple__action__client_1_1SimpleActionClient.html # print client.get_state() def move_from_pregrasp_to_grasp(self, second): if self.verbose: print 'Moving from pregrasp to grasp' # Cartesian path header = Header() header.frame_id = "/" + self.reference_frame start_state = RobotState() start_state.joint_state.header.frame_id = "/" + self.reference_frame names = [] vals = []
def __init__(self): joint_state_topic = ['joint_states:=/iiwa/joint_states'] moveit_commander.roscpp_initialize(joint_state_topic) rospy.init_node('moveit_ik_demo') arm = moveit_commander.MoveGroupCommander('arm') gripper = moveit_commander.MoveGroupCommander('gripper') end_effector_link = arm.get_end_effector_link() reference_frame = 'iiwa_link_0' arm.set_pose_reference_frame(reference_frame) gripper_joints = ['finger_joint'] arm.allow_replanning(True) # ----set gripper action client---- gripper_client = actionlib.SimpleActionClient( 'iiwa/gripper_controller/follow_joint_trajectory', FollowJointTrajectoryAction) gripper_client.wait_for_server() rospy.loginfo('...connected.') # ----set accuracy---- arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.01) gripper.set_goal_position_tolerance(0.01) gripper.set_goal_orientation_tolerance(0.05) # ----set arm poses---- # pose1 target_pose1 = PoseStamped() target_pose1.header.frame_id = reference_frame target_pose1.header.stamp = rospy.Time.now() target_pose1.pose.position.x = 0.6 target_pose1.pose.position.y = 0.0 target_pose1.pose.position.z = 0.3 target_pose1.pose.orientation.x = 0 target_pose1.pose.orientation.y = 1 target_pose1.pose.orientation.z = 0 target_pose1.pose.orientation.w = 0 # pose1 target_pose2 = PoseStamped() target_pose2.header.frame_id = reference_frame target_pose2.header.stamp = rospy.Time.now() target_pose2.pose.position.x = 0.0 target_pose2.pose.position.y = 0.6 target_pose2.pose.position.z = 0.3 target_pose2.pose.orientation.x = 0 target_pose2.pose.orientation.y = 1 target_pose2.pose.orientation.z = 0 target_pose2.pose.orientation.w = 0 # ----set gripepr poses---- gripper_open = [0.05] gripper_close = [0.5] gripper_traj_open = JointTrajectory() gripper_traj_open.joint_names = gripper_joints gripper_traj_open.points.append(JointTrajectoryPoint()) gripper_traj_open.points[0].positions = gripper_open gripper_traj_open.points[0].velocities = [0.0 for i in gripper_joints] gripper_traj_open.points[0].accelerations = [ 0.0 for i in gripper_joints ] gripper_traj_open.points[0].time_from_start = rospy.Duration(3.0) gripper_traj_close = JointTrajectory() gripper_traj_close.joint_names = gripper_joints gripper_traj_close.points.append(JointTrajectoryPoint()) gripper_traj_close.points[0].positions = gripper_close gripper_traj_close.points[0].velocities = [0.0 for i in gripper_joints] gripper_traj_close.points[0].accelerations = [ 0.0 for i in gripper_joints ] gripper_traj_close.points[0].time_from_start = rospy.Duration(3.0) gripper_goal = FollowJointTrajectoryGoal() gripper_goal.trajectory = gripper_traj_open gripper_goal.goal_time_tolerance = rospy.Duration(0.0) gripper_client.send_goal(gripper_goal) gripper_client.wait_for_result(rospy.Duration(5.0)) # other setting shift_value = -0.05 while (True): arm.set_named_target('home') arm.go() rospy.sleep(2) # arm pose1 arm.set_start_state_to_current_state() arm.set_pose_target(target_pose1, end_effector_link) traj = arm.plan() arm.execute(traj) rospy.sleep(1) # move close to target arm.shift_pose_target(2, shift_value, end_effector_link) arm.go() rospy.sleep(1) # close griper gripper_goal = FollowJointTrajectoryGoal() gripper_goal.trajectory = gripper_traj_close gripper_goal.goal_time_tolerance = rospy.Duration(0.0) gripper_client.send_goal(gripper_goal) gripper_client.wait_for_result(rospy.Duration(5.0)) # arm pose2 arm.set_start_state_to_current_state() arm.set_pose_target(target_pose2, end_effector_link) traj = arm.plan() arm.execute(traj) rospy.sleep(1) # move close to target arm.shift_pose_target(2, shift_value, end_effector_link) arm.go() rospy.sleep(1) # open griper gripper_goal = FollowJointTrajectoryGoal() gripper_goal.trajectory = gripper_traj_open gripper_goal.goal_time_tolerance = rospy.Duration(0.0) gripper_client.send_goal(gripper_goal) gripper_client.wait_for_result(rospy.Duration(5.0)) arm.set_named_target('home') arm.go() rospy.sleep(2) """ arm.shift_pose_target(1, -0.05, end_effector_link) arm.go() rospy.sleep(1) arm.shift_pose_target(3, -1.57, end_effector_link) arm.go() rospy.sleep(1) arm.set_named_target('home') arm.go() """ moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
print("Generating client...") client = actionlib.SimpleActionClient('left_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) print("Waiting for server...") client.wait_for_server() # create goal goal = FollowJointTrajectoryGoal() # define goal trajectory waypoint1 = JointTrajectoryPoint() waypoint1.positions = [0, 1, .75, 0.5] waypoint1.time_from_start = Duration(0.0) traj1 = JointTrajectory() traj1.joint_names = ["l_shoulder_y", "l_shoulder_x", "l_arm_z", "l_elbow_y"] traj1.points = [waypoint1] # set goal trajectory goal.trajectory = traj1 # send goal client.send_goal(goal, feedback_cb=feedback_cb) print("Sent goal") client.wait_for_result() print('[Result] State: %d'%(client.get_state())) time.sleep(15)