def createHandGoal(side, j1, j2, j3): """Creates a FollowJointTrajectoryGoal with the values specified in j1, j2 and j3 for the joint positions with the hand specified in side @arg side string 'right' or 'left' @arg j1 float value for joint 'hand_'+side+'_thumb_joint' @arg j2 float value for joint 'hand_'+side+'_middle_joint' @arg j3 float value for joint 'hand_'+side+'_index_joint' @return FollowJointTrajectoryGoal with the specified goal""" fjtg = FollowJointTrajectoryGoal() fjtg.trajectory.joint_names.append('hand_'+side+'_thumb_joint') fjtg.trajectory.joint_names.append('hand_'+side+'_middle_joint') fjtg.trajectory.joint_names.append('hand_'+side+'_index_joint') point = JointTrajectoryPoint() point.positions.append(j1) point.positions.append(j2) point.positions.append(j3) point.velocities.append(0.0) point.velocities.append(0.0) point.velocities.append(0.0) point.time_from_start = rospy.Duration(4.0) fjtg.trajectory.points.append(point) for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware goal_tol = JointTolerance() goal_tol.name = joint goal_tol.position = 5.0 goal_tol.velocity = 5.0 goal_tol.acceleration = 5.0 fjtg.goal_tolerance.append(goal_tol) fjtg.goal_time_tolerance = rospy.Duration(3) fjtg.trajectory.header.stamp = rospy.Time.now() return fjtg
def __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_arm_joints(client, arm_joint_positions, duration=5.0): trajectory = JointTrajectory() trajectory.joint_names = ARM_JOINT_NAMES trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = arm_joint_positions trajectory.points[0].velocities = [0.0] * len(arm_joint_positions) trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions) trajectory.points[0].time_from_start = rospy.Duration(duration) arm_goal = FollowJointTrajectoryGoal() arm_goal.trajectory = trajectory arm_goal.goal_time_tolerance = rospy.Duration(0.0) print("Moving amr to joints: ", arm_joint_positions) client.send_goal(arm_goal) client.wait_for_result(rospy.Duration(duration+1.0)) rospy.loginfo("...done")
def __init__(self): rospy.init_node('trajectory_demo') # 是否需要回到初始位置 reset = rospy.get_param('~reset', False) arm_joints = [ 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6' ] # 调整目标位置 if reset: arm_goal = [0, 0, 0, 0, 0, 0] else: arm_goal = [-0.3, -1.0, 0.5, 0.8, 1.0, -0.7] # 连接机械臂轨迹规划的trajectory action server rospy.loginfo('waiting for arm trajectory controller...') arm_client = actionlib.SimpleActionClient( 'arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) arm_client.wait_for_server() rospy.loginfo('...connected.') arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].velocities = [0.0 for i in arm_joints] arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(3.0) rospy.loginfo('Moving the arm to goal position...') arm_goal = FollowJointTrajectoryGoal() arm_goal.trajectory = arm_trajectory arm_goal.goal_time_tolerance = rospy.Duration(0.0) arm_client.send_goal(arm_goal) arm_client.wait_for_result(rospy.Duration(5.0)) rospy.loginfo('...done')
def tiltWristGoal(beginPositions, endPositions): trajectory = JointTrajectory() trajectory.joint_names = armJointNames trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = beginPositions trajectory.points[0].velocities = [0.0] * len(beginPositions) trajectory.points[0].accelerations = [0.0] * len(beginPositions) trajectory.points[0].time_from_start = rospy.Duration(0.0) trajectory.points.append(JointTrajectoryPoint()) trajectory.points[1].positions = endPositions trajectory.points[1].velocities = [0.0] * len(endPositions) trajectory.points[1].accelerations = [0.0] * len(endPositions) trajectory.points[1].time_from_start = rospy.Duration(2.0) armGoal = FollowJointTrajectoryGoal() armGoal.trajectory = trajectory armGoal.goal_time_tolerance = rospy.Duration(0.0) return armGoal
def publish_goal(): action_client = True client = actionlib.SimpleActionClient( '/revel_trajectory_controller/follow_joint_trajectory', FollowJointTrajectoryAction) pub = rospy.Publisher('/revel_trajectory_controller/command', JointTrajectory, queue_size=10) rospy.init_node('trajectory_tester', anonymous=True) traj = JointTrajectory() traj.joint_names.append("joint_1") traj.joint_names.append("joint_2") traj.joint_names.append("joint_3") traj.joint_names.append("joint_4") traj.joint_names.append("joint_5") traj.joint_names.append("joint_6") jtp = JointTrajectoryPoint() jtp.positions.append(0.0) jtp.positions.append(0.0) jtp.positions.append(0.0) jtp.positions.append(0.0) jtp.positions.append(0.0) jtp.positions.append(0.0) jtp.velocities.append(1.0) jtp.velocities.append(1.0) jtp.velocities.append(1.0) jtp.velocities.append(1.0) jtp.velocities.append(1.0) jtp.velocities.append(1.0) jtp.time_from_start = rospy.Duration(0.0) traj.points.append(jtp) traj.header.stamp = rospy.Time.now() if not action_client: pub.publish(traj) else: goal = FollowJointTrajectoryGoal() goal.trajectory = traj goal.goal_time_tolerance = rospy.Duration(4.0) client.wait_for_server() print 'Found server' client.send_goal(goal)
def execute_trajectory(path): #Creates the SimpleActionClient, passing the type of the action client = actionlib.SimpleActionClient('joint_trajectory_action', FollowJointTrajectoryAction) #Waits until the action server has started up and started listening for goals. client.wait_for_server() #Creates a goal to send to the action server. goal = FollowJointTrajectoryGoal() goal.trajectory = path goal.goal_time_tolerance = rospy.Duration(0.01) #Sends the goal to the action server. client.send_goal(goal) #Waits for the server to finish performing the action. client.wait_for_result() #Returns out the result of executing the action return client.get_result()
def brazo(): print("Initializing node... ") #rospy.init_node("joint_trajectory_client_example") rospy.sleep(1) print("Running. Ctrl-c to quit") positions = { 'larm_shoulder_p': -1.55, 'larm_shoulder_r': 1.55, 'larm_elbow_p': -1.0, 'larm_gripper': 0.0, } client = actionlib.SimpleActionClient( '/fullbody_controller/follow_joint_trajectory', FollowJointTrajectoryAction, ) if not client.wait_for_server(timeout=rospy.Duration(10)): rospy.logerr("Timed out waiting for Action Server") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1) # init goal goal = FollowJointTrajectoryGoal() goal.goal_time_tolerance = rospy.Time(1) goal.trajectory.joint_names = positions.keys() # points point = JointTrajectoryPoint() goal.trajectory.joint_names = positions.keys() point.positions = positions.values() point.time_from_start = rospy.Duration(10) goal.trajectory.points.append(point) # send goal goal.trajectory.header.stamp = rospy.Time.now() client.send_goal(goal) print(goal) print("waiting...") if not client.wait_for_result(timeout=rospy.Duration(20)): rospy.logerr("Timed out waiting for JTA") rospy.loginfo("Exitting...")
def test_scaled_trajectory(self): """Test robot movement.""" goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [ "elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint", ] position_list = [[0.0 for i in range(6)]] position_list.append([-1.0 for i in range(6)]) duration_list = [6.0, 6.5] for i, position in enumerate(position_list): point = JointTrajectoryPoint() point.positions = position point.time_from_start = rospy.Duration(duration_list[i]) goal.trajectory.points.append(point) rospy.loginfo("Sending scaled goal without time restrictions") self.client.send_goal(goal) self.client.wait_for_result() self.assertEqual( self.client.get_result().error_code, FollowJointTrajectoryResult.SUCCESSFUL ) rospy.loginfo("Received result SUCCESSFUL") # Now do the same again, but with a goal time constraint rospy.loginfo("Sending scaled goal with time restrictions") goal.goal_time_tolerance = rospy.Duration(0.01) self.client.send_goal(goal) self.client.wait_for_result() self.assertEqual( self.client.get_result().error_code, FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED ) rospy.loginfo("Received result GOAL_TOLERANCE_VIOLATED")
def setHeadTilt(self): head_joint_names = ["head_pan_joint", "head_tilt_joint"] head_joint_positions = [0.0, 0.4] 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 for _ in head_joint_positions] trajectory.points[0].accelerations = [ 0.0 for _ in head_joint_positions] trajectory.points[0].time_from_start = rospy.Duration(0.0) head_goal = FollowJointTrajectoryGoal() head_goal.trajectory = trajectory head_goal.goal_time_tolerance = rospy.Duration(0.0) rospy.loginfo("Setting Head positions...") self.head_client.send_goal(head_goal) self.head_client.wait_for_result(rospy.Duration(10.0)) rospy.loginfo("...done")
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 test_joint_passthrough(self): #### Power cycle the robot in order to make sure it is running correctly#### self.assertTrue(self.set_robot_to_mode(RobotMode.POWER_OFF)) rospy.sleep(0.5) self.assertTrue(self.set_robot_to_mode(RobotMode.RUNNING)) rospy.sleep(0.5) self.send_program_srv.call() rospy.sleep(0.5) # TODO properly wait until the controller is running self.switch_on_controller("forward_joint_traj_controller") goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [ "elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] position_list = [[0, -1.57, -1.57, 0, 0, 0]] position_list.append([0.2, -1.57, -1.57, 0, 0, 0]) position_list.append([-0.5, -1.57, -1.2, 0, 0, 0]) duration_list = [3.0, 7.0, 10.0] for i, position in enumerate(position_list): point = JointTrajectoryPoint() point.positions = position point.time_from_start = rospy.Duration(duration_list[i]) goal.trajectory.points.append(point) for i, joint_name in enumerate(goal.trajectory.joint_names): goal.goal_tolerance.append( JointTolerance(joint_name, 0.2, 0.2, 0.2)) goal.goal_time_tolerance = rospy.Duration(0.6) self.joint_passthrough_trajectory_client.send_goal(goal) self.joint_passthrough_trajectory_client.wait_for_result() self.assertEqual( self.joint_passthrough_trajectory_client.get_result().error_code, FollowJointTrajectoryResult.SUCCESSFUL) rospy.loginfo("Received result SUCCESSFUL")
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 __init__(self): rospy.init_node('trajectory_demo') reset = rospy.get_param('~reset', False) arm_joints = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ] if reset: arm_goal = [0, 0, 0, 0, 0, 0] else: arm_goal = [0.09, 0.26, 1.02, 0, -1.14, 3.24] rospy.loginfo('Waiting for arm trajectory controller...') arm_client = actionlib.SimpleActionClient('/joint_trajectory_action', FollowJointTrajectoryAction) arm_client.wait_for_server() rospy.loginfo('...connected.') arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].velocities = [0.0 for i in arm_joints] arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(3.0) rospy.loginfo('Moving the arm to goal position...') goal = FollowJointTrajectoryGoal() goal.trajectory = arm_trajectory goal.goal_time_tolerance = rospy.Duration(0.0) arm_client.send_goal(goal) arm_client.wait_for_result(rospy.Duration(5.0)) rospy.loginfo('...done')
def main(filename): print("Initializing node... ") #rospy.init_node("joint_trajectory_client_csv_example") rospy.sleep(1) print("Running. Ctrl-c to quit") # init goal goal = FollowJointTrajectoryGoal() goal.goal_time_tolerance = rospy.Time(1) with open(filename) as f: reader = csv.reader(f, skipinitialspace=True) first_row = True for row in reader: if first_row: goal.trajectory.joint_names = row[1:] first_row = False else: point = JointTrajectoryPoint() point.positions = [float(n) for n in row[1:]] point.time_from_start = rospy.Duration(float(row[0])) goal.trajectory.points.append(point) client = actionlib.SimpleActionClient( '/fullbody_controller/follow_joint_trajectory', FollowJointTrajectoryAction, ) if not client.wait_for_server(timeout=rospy.Duration(10)): rospy.logerr("Timed out waiting for Action Server") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1) # send goal goal.trajectory.header.stamp = rospy.Time.now() client.send_goal(goal) print("waiting...") if not client.wait_for_result(timeout=rospy.Duration(60)): rospy.logerr("Timed out waiting for JTA") rospy.loginfo("Exitting...")
def move_arm_joints_v2(client, arm_joint_positions_list): trajectory = JointTrajectory() trajectory.joint_names = ARM_JOINT_NAMES duration = 0.0 wait_time = np.random.uniform(3.0, 5.0) for i, a_joint_pose in enumerate(arm_joint_positions_list): duration += np.random.uniform(3.0, 5.0) trajectory.points.append(JointTrajectoryPoint()) trajectory.points[i].positions = a_joint_pose trajectory.points[i].velocities = [0.0] * len(a_joint_pose) trajectory.points[i].accelerations = [0.0] * len(a_joint_pose) 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) print("Moving amr to joints: ", arm_joint_positions_list) client.send_goal(arm_goal) client.wait_for_result(rospy.Duration(len(arm_joint_positions_list) * wait_time)) rospy.loginfo("...done")
def execute(self, trajectory, done_cb=None, active_cb=None, feedback_cb=None): """ :type trajectory: trajectory_msgs/JointTrajectory :type done_cb: function :type active_cb: function :type feedback_cb: function """ if not self._connected: rospy.logwarn_throttle( 1, "TrajectoryAsyncExecuter: Client failed to connect to the action server .. nothing will be executed!" ) return False goal = FollowJointTrajectoryGoal() goal.goal_time_tolerance = self._goal_time_tolerance goal.trajectory = trajectory self._client.send_goal(goal, done_cb, active_cb, feedback_cb)
def __init__(self): rospy.init_node('trajectory_demo') reset = rospy.get_param('~reset', False) arm_joints = [ 'base_to_armA', 'armA_to_armB', 'armB_to_armC', 'armC_to_armD' ] if reset: arm_goal = [0, 0, 0, 0] else: arm_goal = [-0.3, -1.0, 0.5, 0.8] rospy.loginfo("Waiting for arm trajectory controller...") arm_client = actionlib.SimpleActionClient( 'arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) arm_client.wait_for_server() rospy.loginfo('...connected.') arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].velocities = [0 for i in arm_joints] arm_trajectory.points[0].accelerations = [0 for i in arm_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(3) rospy.loginfo("Moving the arm to goal position..") arm_goal = FollowJointTrajectoryGoal() arm_goal.trajectory = arm_trajectory arm_goal.goal_time_tolerance = rospy.Duration(0) arm_client.send_goal(arm_goal) arm_client.wait_for_result(rospy.Duration(5)) rospy.loginfo('Done.')
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 goal_gripper(self, pos): if pos == "close": self.a2 = -1.6 self.b2 = 1.6 else: self.a2 = 0 self.b2 = 0 trajectory = JointTrajectory() trajectory.joint_names = self.joint_names1 trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = [self.a2, self.b2] trajectory.points[0].time_from_start = rospy.Duration(0.001) goal = FollowJointTrajectoryGoal() goal.trajectory = trajectory goal.goal_time_tolerance = rospy.Duration(0.0) self.client1.send_goal(goal) while True: if abs(self.g_0 - self.a2) < 0.01 and abs(self.h_0 - self.b2) < 0.01: break
def main(): jta = actionlib.SimpleActionClient('/arm_1/arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) jta.wait_for_server() goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = ['arm_joint_1', 'arm_joint_2','arm_joint_3','arm_joint_4', 'arm_joint_5'] goal.goal_time_tolerance = rospy.Time(0.3) point = [] for i in range(0, 4): point.append(JointTrajectoryPoint()) point[0].positions =[0.1, 0.1, -0.1, 0.1, 0.1] point[0].velocities = [0,0,0,0,0] point[0].accelerations = [0,0,0,0,0] point[0].time_from_start = rospy.Duration(0.0) goal.trajectory.points.append(point[0]) point[1].positions =[3.14, 1.57, -1.57, 1.57, 1.57] point[1].velocities = [0,0,0,0,0] point[1].accelerations = [0,0,0,0,0] point[1].time_from_start = rospy.Duration(6) goal.trajectory.points.append(point[1]) point[2].positions =[2, 2, -2, 2, 2] point[2].velocities = [0,0,0,0,0] point[2].accelerations = [0,0,0,0,0] point[2].time_from_start = rospy.Duration(12.0) goal.trajectory.points.append(point[2]) point[3].positions =[0.4, 0.4, -0.4, 0.4, 0.4] point[3].velocities = [0,0,0,0,0] point[3].accelerations = [0,0,0,0,0] point[3].time_from_start = rospy.Duration(18.0) goal.trajectory.points.append(point[3]) jta.send_goal_and_wait(goal)
def setArmJoints(self, joint_states, joint_vel): 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_joint_positions = joint_states arm_joint_velocities = joint_vel trajectory = JointTrajectory() trajectory.joint_names = arm_joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = arm_joint_positions trajectory.points[0].velocities = arm_joint_velocities trajectory.points[0].accelerations = [ 0.0 for _ in arm_joint_velocities] trajectory.points[0].time_from_start = rospy.Duration(0.0001) arm_goal = FollowJointTrajectoryGoal() arm_goal.trajectory = trajectory arm_goal.goal_time_tolerance = rospy.Duration(25.0) rospy.loginfo("Setting Arm positions...") self.arm_client.send_goal(arm_goal) # self.arm_client.wait_for_result(rospy.Duration(25.0)) rospy.loginfo("...done")
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 receiverLoop(self): while True: print "self.conn.recv()" input_msg = self.conn.recv() print input_msg goal = FollowJointTrajectoryGoal() goal.goal_time_tolerance = input_msg.goal_time_tolerance goal.goal_tolerance = input_msg.goal_tolerance goal.path_tolerance = input_msg.path_tolerance goal.trajectory.header = input_msg.trajectory.header goal.trajectory.joint_names = input_msg.joint_names for input_point in input_msg.trajectory.points: p = JointTrajectoryPoint() p.accelerations = input_point.accelerations p.positions = input_point.positions p.velocities = input_point.velocities p.time_from_start = input_point.time_from_start goal.trajectory.points.append(p) print "Goal will be:" print goal #self.right_arm_as.send_goal(goal) rospy.loginfo("Waiting for result") self.right_arm_as.wait_for_result() rospy.loginfo("Goal done")
def move_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))
def gripper_final_action(self): self.timer = 0 print "in gripper_final_action... " service_result = self.joint_config_client(0, 0, 0.0, 1.0) # print "sevice ", service_result pose_difference = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -0.22], [0, 0, 0, 1]]) final_pose_T = np.dot(self.base_target_T, pose_difference) # block of interpolation between current_pose and final_pose of ee print "final_pose_T : ", final_pose_T final_q_ik = self.kdl_kin.inverse( final_pose_T, np.array(self.end_eff_current_pose) + 0.3) # print "final_q_ik : ", np.shape(final_q_ik), type(final_q_ik) if final_q_ik is None: print "The target could not be reached!" return # actionlib commands self.trajectory.points[0].positions = final_q_ik 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) goal = FollowJointTrajectoryGoal() goal.trajectory = self.trajectory goal.goal_time_tolerance = rospy.Duration(0.0) self.manipulator_client.send_goal(goal) self.manipulator_client.wait_for_result() service_result = self.joint_config_client(0, 0, 1.0, 1.0)
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[2].positions = arm_joint_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) # 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')
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')
def marker_inf_cb(self, data): print "in marker callback" q = Quaternion() q = data.pose.orientation marker_orientation = np.array([q.x, q.y, q.z, q.w]) euler_angles = tr.euler_from_quaternion(marker_orientation, 'rxyz') vec3 = Vector3() vec3.x = data.pose.position.x vec3.y = data.pose.position.y vec3.z = data.pose.position.z marker_pose = np.array([ vec3.x, vec3.y, vec3.z, euler_angles[0], euler_angles[1], euler_angles[2] ]) for i in range(6): if np.isnan(marker_pose[i]): return cam_object_T = self.set_cam_object_T(marker_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) # print base_object_T 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.5 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 __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): 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')