def interpolate_joint_space(self, goal, total_time, nb_points, start=None): """ Interpolate a trajectory from a start state (or current state) to a goal in joint space :param goal: A RobotState to be used as the goal of the trajectory param total_time: The time to execute the trajectory :param nb_points: Number of joint-space points in the final trajectory :param start: A RobotState to be used as the start state, joint order must be the same as the goal :return: The corresponding RobotTrajectory """ dt = total_time*(1.0/nb_points) # create the joint trajectory message traj_msg = JointTrajectory() rt = RobotTrajectory() if start == None: start = self.get_current_state() joints = [] start_state = start.joint_state.position goal_state = goal.joint_state.position for j in range(len(goal_state)): pose_lin = np.linspace(start_state[j],goal_state[j],nb_points+1) joints.append(pose_lin[1:].tolist()) for i in range(nb_points): point = JointTrajectoryPoint() for j in range(len(goal_state)): point.positions.append(joints[j][i]) # append the time from start of the position point.time_from_start = rospy.Duration.from_sec((i+1)*dt) # append the position to the message traj_msg.points.append(point) # put name of joints to be moved traj_msg.joint_names = self.joint_names() # send the message rt.joint_trajectory = traj_msg return rt
def test_joint_trajectory_action(self): larm = actionlib.SimpleActionClient("/fullbody_controller/joint_trajectory_action", JointTrajectoryAction) larm.wait_for_server() try: self.listener.waitForTransform('/BASE_LINK', '/J7_LINK', rospy.Time(), rospy.Duration(120)) except tf.Exception: self.assertTrue(None, "could not found tf from /BASE_LINK to /J7_LINK") (trans1,rot1) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0)) rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans1,rot1)) goal = JointTrajectoryGoal() goal.trajectory.header.stamp = rospy.get_rostime() goal.trajectory.joint_names.append("J1") goal.trajectory.joint_names.append("J2") goal.trajectory.joint_names.append("J3") goal.trajectory.joint_names.append("J4") goal.trajectory.joint_names.append("J5") goal.trajectory.joint_names.append("J6") goal.trajectory.joint_names.append("J7") point = JointTrajectoryPoint() point.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ] point.time_from_start = rospy.Duration(5.0) goal.trajectory.points.append(point) larm.send_goal(goal) larm.wait_for_result() (trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0)) rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2)) rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2)))) self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
def createHeadGoal(self): (roll, pitch, yaw) = euler_from_quaternion([self.last_oculus_msg.pose.orientation.x, self.last_oculus_msg.pose.orientation.y, self.last_oculus_msg.pose.orientation.z, self.last_oculus_msg.pose.orientation.w]) rospy.loginfo("roll, pitch, yaw: ") rospy.loginfo(str(roll) + " " + str(pitch) + " " + str(yaw)) head_goal = FollowJointTrajectoryGoal() head_goal.trajectory.joint_names.append('head_1_joint') # 1 positivo izquierda, -1 derecha head_goal.trajectory.joint_names.append('head_2_joint') # -1 arriba, 1 abajo # Current position trajectory point jtp_current = JointTrajectoryPoint() jtp_current.positions.append(self.last_head_state.actual.positions[0]) jtp_current.positions.append(self.last_head_state.actual.positions[1]) jtp_current.velocities.append(self.last_head_state.actual.velocities[0]) jtp_current.velocities.append(self.last_head_state.actual.velocities[1]) jtp_current.time_from_start = rospy.Duration(0.0) # Next goal position jtp = JointTrajectoryPoint() jtp.positions.append(yaw *-1) jtp.positions.append(pitch *-1) # jtp.positions.append((yaw *-1) + 1.5707963267948966) # + 180 deg # jtp.positions.append((roll + 1.5707963267948966) *-1) # + 180 deg because of torso2 jtp.velocities.append(0.0) jtp.velocities.append(0.0) rospy.loginfo("Sending: " + str(jtp.positions)) #jtp.time_from_start.secs = 1 jtp.time_from_start.nsecs = 100 head_goal.trajectory.points.append(jtp_current) head_goal.trajectory.points.append(jtp) head_goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) return head_goal
def control_loop(self): if self.commands is None: raise Exception('Command list is empty. Was the control plan loaded?') # loop through the command list for cmd in self.commands: # wait for proxy to be in active state self.wait_for_state(self._proxy_state_running) # process commands cmd_spec_str = None spec = None t = cmd[ProxyCommand.key_command_type] if not (t == "noop"): cmd_spec_str = cmd[ProxyCommand.key_command_spec] if not isinstance(cmd_spec_str, basestring): spec = float(cmd_spec_str) else: spec = self.positions[cmd_spec_str] rospy.loginfo("Command type: " + t + ", spec: " + str(cmd_spec_str) + ", value: " + str(spec)) # check for any wait depends self.wait_for_depend(cmd) # execute command # could do this with a dictionary-based function lookup, but who cares if t == 'noop': rospy.loginfo("Command type: noop") self.wait_for_depend(cmd) elif t == 'sleep': rospy.loginfo("sleep command") v = float(spec) rospy.sleep(v) elif t == 'move_gripper': rospy.loginfo("gripper command") self.move_gripper(spec) elif t == 'move_arm': rospy.loginfo("move_arm command") rospy.logdebug(spec) goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = self.arm_joint_names jtp = JointTrajectoryPoint() jtp.time_from_start = rospy.Duration(0.5) # fudge factor for gazebo controller jtp.positions = spec jtp.velocities = [0]*len(spec) goal.trajectory.points.append(jtp) self._arm_goal = copy.deepcopy(goal) self.move_arm() elif t == 'plan_exec_arm': rospy.loginfo("plan and execute command not implemented") raise NotImplementedError() else: raise Exception("Invalid command type: " + str(cmd.type)) # check for any set dependencies action self.set_depend(cmd) # check for any clear dependencies action self.clear_depend(cmd)
def __init__(self, arm = 'l', record_data = False): self.arm = arm if arm == 'r': print "using right arm on Darci" self.RIGHT_ARM = 0 # i don't think this was used anywhere else: self.LEFT_ARM = 1 # print "Left and both arms not implemented in this client yet ... \n" # print "will require writing different function calls since joint data for left and right arm come in together from the meka server" # sys.exit() self.kinematics = dak.DarciArmKinematics(arm) self.joint_pub = rospy.Publisher('/'+self.arm+'_arm_controller/command', JointTrajectory) self.joint_names = None self.lock = RLock() self.joint_angles = None self.joint_velocities = None self.J_h = None self.time = None self.desired_joint_angles = None self.stiffness_percent = 0.75 self.ee_force = None self.ee_torque = None self.skins = None #will need code for all of these skin interfaces self.Jc_l = [] self.n_l = [] self.values_l = [] #values from m3gtt repository in robot_config folder # These could be read in from a yaml file like this (import yaml; stream = open("FILE_NAME_HERE", 'r'); data = yaml.load(stream)) # However, not clear if absolute path to yaml file (possibly on another computer) is better then just defining it here # The downside is obviously if someone tunes gains differently on the robot. self.joint_stiffness = (np.array([1, 1, 1, 1, 0.06, 0.08, 0.08])*180/np.pi*self.stiffness_percent).tolist() self.joint_damping = (np.array([0.06, 0.1, 0.015, 0.015, 0.0015, 0.002, 0.002])*180/np.pi*self.stiffness_percent).tolist() self.record_data = record_data if self.record_data: from collections import deque self.q_record = deque() self.qd_record = deque() self.times = deque() self.state_sub = rospy.Subscriber('/'+self.arm+'_arm_controller/state', JointTrajectoryControllerState, self.robotStateCallback) rospy.sleep(1.0) while self.joint_angles is None: rospy.sleep(0.05) self.desired_joint_angles = copy.copy(self.joint_angles) self.joint_cmd = JointTrajectory() self.joint_cmd.header.stamp = rospy.Time.now() self.joint_cmd.header.frame_id = '/torso_lift_link' self.joint_cmd.joint_names = self.joint_names jtp = JointTrajectoryPoint() jtp.positions = self.desired_joint_angles jtp.velocities = [1.]*len(self.joint_names) self.joint_cmd.points = [jtp] self.joint_pub.publish(self.joint_cmd)
def convert_trajectory(self, traj): """ Converts a trajectory into a joint trajectory Args: traj: Trajectory to convert Returns: joint trajectory """ new_traj = JointTrajectory() new_traj.header = traj.header # Take each joint in the trajectory and add it to the joint trajectory for joint_name in traj.joint_names: new_traj.joint_names.append(self.joint_map[joint_name].joint_name) # For each poiint in the trajectory for point in traj.points: new_point = JointTrajectoryPoint() for i, pos in enumerate(point.positions): new_point.positions.append(self.joint_map[new_traj.joint_names[i]].convert_angle(pos)) for i, vel in enumerate(point.velocities): new_point.velocities.append(self.joint_map[new_traj.joint_names[i]].convert_velocity(vel)) new_point.time_from_start = point.time_from_start new_traj.points.append(new_point) return new_traj
def add_point(self, positions, velocities, accelerations, time): point = JointTrajectoryPoint() point.positions = copy(positions) point.velocities = copy(velocities) point.accelerations = copy(accelerations) point.time_from_start = rospy.Duration(time) self._goal.trajectory.points.append(point)
def create_placings_from_object_pose(self, posestamped): """ Create a list of PlaceLocation of the object rotated every 15deg""" place_locs = [] pre_grasp_posture = JointTrajectory() # Actually ignored.... pre_grasp_posture.header.frame_id = self._grasp_pose_frame_id pre_grasp_posture.joint_names = [ name for name in self._gripper_joint_names.split()] jtpoint = JointTrajectoryPoint() jtpoint.positions = [ float(pos) for pos in self._gripper_pre_grasp_positions.split()] jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture) pre_grasp_posture.points.append(jtpoint) # Generate all the orientations every step_degrees_yaw deg for yaw_angle in np.arange(0.0, 2.0 * pi, radians(self._step_degrees_yaw)): pl = PlaceLocation() pl.place_pose = posestamped newquat = quaternion_from_euler(0.0, 0.0, yaw_angle) pl.place_pose.pose.orientation = Quaternion( newquat[0], newquat[1], newquat[2], newquat[3]) # TODO: the frame is ignored, this will always be the frame of the gripper # so arm_tool_link pl.pre_place_approach = self.createGripperTranslation( Vector3(1.0, 0.0, 0.0)) pl.post_place_retreat = self.createGripperTranslation( Vector3(-1.0, 0.0, 0.0)) pl.post_place_posture = pre_grasp_posture place_locs.append(pl) return place_locs
def test_5(self): # Test 5: move each joint in robotic arm to a specific value at the same time (mixed speeds) rospy.loginfo("Begin test 5") # Create a joint state publisher self.arm_pub = rospy.Publisher('arm_controller/command', JointTrajectory) rospy.sleep(1) # How fast will we update the robot's movement? in Hz rate = 10 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Define a joint trajectory message for grasp position rospy.loginfo("Position arm in stowed position") msg = JointTrajectory() msg.header.seq = 1 msg.header.stamp = rospy.Time.now() msg.header.frame_id = '0' msg.joint_names.append('joint_1') msg.joint_names.append('joint_2') msg.joint_names.append('joint_3') msg.joint_names.append('joint_4') msg.joint_names.append('joint_5') # Define joint trajectory points point = JointTrajectoryPoint() point.positions = [0.0, -1.3, 1.7, 1.2, 0.1] point.velocities = [90.0, 50.0, 20.0, 10.0, 10.0] point.time_from_start = rospy.Duration(3.0) msg.points.append(point) # publish joint stage message self.arm_pub.publish(msg) rospy.loginfo("End test 5")
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 states_to_trajectory(states, time_step=0.1): """ Converts a list of RobotState/JointState in Robot Trajectory :param robot_states: list of RobotState :param time_step: duration in seconds between two consecutive points :return: a Robot Trajectory """ rt = RobotTrajectory() for state_idx, state in enumerate(states): if isinstance(state, RobotState): state = state.joint_state jtp = JointTrajectoryPoint() jtp.positions = state.position # jtp.velocities = state.velocity # Probably does not make sense to keep velocities and efforts here # jtp.effort = state.effort jtp.time_from_start = rospy.Duration(state_idx*time_step) rt.joint_trajectory.points.append(jtp) if len(states) > 0: if isinstance(states[0], RobotState): rt.joint_trajectory.joint_names = states[0].joint_state.name else: rt.joint_trajectory.joint_names = states[0].joint_names return rt
def tuck(self): # prepare a joint trajectory msg = JointTrajectory() msg.joint_names = servos msg.points = list() point = JointTrajectoryPoint() point.positions = forward point.velocities = [ 0.0 for servo in msg.joint_names ] point.time_from_start = rospy.Duration(5.0) msg.points.append(point) # point = JointTrajectoryPoint() #point.positions = to_side #point.velocities = [ 0.0 for servo in msg.joint_names ] #point.time_from_start = rospy.Duration(8.0) #msg.points.append(point) #point = JointTrajectoryPoint() #point.positions = tucked #point.velocities = [ 0.0 for servo in msg.joint_names ] #point.time_from_start = rospy.Duration(11.0) #msg.points.append(point) # call action msg.header.stamp = rospy.Time.now() + rospy.Duration(0.1) goal = FollowJointTrajectoryGoal() goal.trajectory = msg self._client.send_goal(goal)
def stage_3(self): # Move arm to grasping position rospy.loginfo("Begin stage 3") # Create a joint state publisher self.arm_pub = rospy.Publisher('arm_controller/command', JointTrajectory) rospy.sleep(1) # How fast will we update the robot's movement? in Hz rate = 10 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Define a joint trajectory message for grasp position rospy.loginfo("Position arm in grasp position") msg = JointTrajectory() msg.header.seq = 1 msg.header.stamp = rospy.Time.now() msg.header.frame_id = '0' msg.joint_names.append('joint_1') msg.joint_names.append('joint_2') msg.joint_names.append('joint_3') msg.joint_names.append('joint_4') msg.joint_names.append('joint_5') # Define joint trajectory points point = JointTrajectoryPoint() point.positions = [0.0, 0.0, 0.0, 1.6, 0.1] point.velocities = [20.0, 20.0, 20.0, 10.0, 10.0] point.time_from_start = rospy.Duration(3.0) msg.points.append(point) # publish joint stage message self.arm_pub.publish(msg) rospy.sleep(3)
def test_trajectory(self): # our goal variable goal = FollowJointTrajectoryGoal() # First, the joint names, which apply to all waypoints goal.trajectory.joint_names = ('shoulder_pitch_joint', 'shoulder_pan_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_pitch_joint', 'wrist_roll_joint') # We will have two waypoints in this goal trajectory # First trajectory point # To be reached 1 second after starting along the trajectory point = JointTrajectoryPoint() point.positions = [0.0] * 7 point.velocities = [1.0] * 7 point.time_from_start = rospy.Duration(3.0) goal.trajectory.points.append(point) #we are done; return the goal return goal
def test_joint_angles(self): larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction) larm.wait_for_server() try: self.listener.waitForTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(), rospy.Duration(120)) except tf.Exception: self.assertTrue(None, "could not found tf from /WAIST_LINK0 to /LARM_LINK7") (trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0)) rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans1,rot1)) goal = JointTrajectoryGoal() goal.trajectory.header.stamp = rospy.get_rostime() goal.trajectory.joint_names.append("LARM_SHOULDER_P") goal.trajectory.joint_names.append("LARM_SHOULDER_R") goal.trajectory.joint_names.append("LARM_SHOULDER_Y") goal.trajectory.joint_names.append("LARM_ELBOW") goal.trajectory.joint_names.append("LARM_WRIST_Y") goal.trajectory.joint_names.append("LARM_WRIST_P") point = JointTrajectoryPoint() point.positions = [ x * math.pi / 180.0 for x in [30,30,30,-90,-40,-30] ] point.time_from_start = rospy.Duration(5.0) goal.trajectory.points.append(point) larm.send_goal(goal) larm.wait_for_result() (trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0)) rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2)) rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2)))) self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
def goal_cb(self, pt_msg): rospy.loginfo("[{0}] New LookatIK goal received.".format(rospy.get_name())) if (pt_msg.header.frame_id != self.camera_ik.base_frame): try: now = pt_msg.header.stamp self.tfl.waitForTransform(pt_msg.header.frame_id, self.camera_ik.base_frame, now, rospy.Duration(1.0)) pt_msg = self.tfl.transformPoint(self.camera_ik.base_frame, pt_msg) except (LookupException, ConnectivityException, ExtrapolationException): rospy.logwarn("[{0}] TF Error tranforming point from {1} to {2}".format(rospy.get_name(), pt_msg.header.frame_id, self.camera_ik.base_frame)) target = np.array([pt_msg.point.x, pt_msg.point.y, pt_msg.point.z]) # Get IK Solution current_angles = list(copy.copy(self.joint_angles_act)) iksol = self.camera_ik.lookat_ik(target, current_angles[:len(self.camera_ik.arm_indices)]) # Start with current angles, then replace angles in camera IK with IK solution # Use desired joint angles to avoid sagging over time jtp = JointTrajectoryPoint() jtp.positions = list(copy.copy(self.joint_angles_des)) for i, joint_name in enumerate(self.camera_ik.arm_joint_names): jtp.positions[self.joint_names.index(joint_name)] = iksol[i] deltas = np.abs(np.subtract(jtp.positions, current_angles)) duration = np.max(np.divide(deltas, self.max_vel_rot)) jtp.time_from_start = rospy.Duration(duration) jt = JointTrajectory() jt.joint_names = self.joint_names jt.points.append(jtp) rospy.loginfo("[{0}] Sending Joint Angles.".format(rospy.get_name())) self.joint_traj_pub.publish(jt)
def createHeadGoal(j1, j2): """Creates a FollowJointTrajectoryGoal with the values specified in j1 and j2 for the joint positions @arg j1 float value for head_1_joint @arg j2 float value for head_2_joint @returns FollowJointTrajectoryGoal with the specified goal""" fjtg = FollowJointTrajectoryGoal() fjtg.trajectory.joint_names.append('head_1_joint') fjtg.trajectory.joint_names.append('head_2_joint') point = JointTrajectoryPoint() point.positions.append(j1) point.positions.append(j2) point.velocities.append(0.0) point.velocities.append(0.0) point.time_from_start = rospy.Duration(4.0) for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware goal_tol = JointTolerance() goal_tol.name = joint goal_tol.position = 5.0 goal_tol.velocity = 5.0 goal_tol.acceleration = 5.0 fjtg.goal_tolerance.append(goal_tol) fjtg.goal_time_tolerance = rospy.Duration(3) fjtg.trajectory.points.append(point) fjtg.trajectory.header.stamp = rospy.Time.now() return fjtg
def runTrajectory(req): global Traj_server; print "---------------------------------" print req.task print " " print req.bin_num print " " print req.using_torso print "---------------------------------" if req.using_torso == "y": file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num); else: file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num); if req.task == "Forward": file_name = file_root+"/forward"; elif req.task == "Drop": file_name = file_root+"/drop"; else : return taskResponse(False); f = open(file_name,"r"); plan_file = RobotTrajectory(); buf = f.read(); plan_file.deserialize(buf); plan = copy.deepcopy(plan_file); arm_right_group = moveit_commander.MoveGroupCommander("arm_right"); arm_left_group = moveit_commander.MoveGroupCommander("arm_left"); arm_left_group.set_start_state_to_current_state(); StartPnt = JointTrajectoryPoint(); StartPnt.positions = arm_left_group.get_current_joint_values(); StartPnt.velocities = [0]*len(StartPnt.positions); StartPnt. accelerations = [0]*len(StartPnt.positions); #print StartPnt; plan.joint_trajectory.points[0] = StartPnt; #print plan; display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory) display_trajectory = moveit_msgs.msg.DisplayTrajectory(); robot = moveit_commander.RobotCommander(); display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while", file_name, " is visualized (again)..." arm_left_group.execute(plan); print "Trajectory ", file_name, " finished!" f.close(); return taskResponse(True);
def joint_trajectory(self, joint_trajectory): ''' Returns just the part of the trajectory corresponding to the arm. **Args:** **joint_trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to convert **Returns:** A trajectory_msgs.msg.JointTrajectory corresponding to only this arm in joint_trajectory **Raises:** **ValueError:** If some arm joint is not found in joint_trajectory ''' arm_trajectory = JointTrajectory() arm_trajectory.header = copy.deepcopy(joint_trajectory.header) arm_trajectory.joint_names = copy.copy(self.joint_names) indexes = [-1]*len(self.joint_names) for (i, an) in enumerate(self.joint_names): for (j, n) in enumerate(joint_trajectory.joint_names): if an == n: indexes[i] = j break if indexes[i] < 0: raise ValueError('Joint '+n+' is missing from joint trajectory') for joint_point in joint_trajectory.points: arm_point = JointTrajectoryPoint() arm_point.time_from_start = joint_point.time_from_start for i in indexes: arm_point.positions.append(joint_point.positions[i]) arm_trajectory.points.append(arm_point) return arm_trajectory
def trajectory_cb(self, traj): rospy.loginfo("Received a new trajectory, beginning the split..") newtraj_arm = JointTrajectory() newtraj_arm.header.stamp = traj.header.stamp newtraj_arm.header.frame_id = traj.header.frame_id for name in traj.joint_names: if name[:5] == "Joint": newtraj_arm.joint_names += (name,) for point in traj.points: newpoint = JointTrajectoryPoint() for i, name in enumerate(traj.joint_names): if name[:5] == "Joint": newpoint.positions += (point.positions[i],) newpoint.velocities += (point.velocities[i],) newpoint.accelerations += (point.accelerations[i],) newpoint.time_from_start = point.time_from_start newtraj_arm.points += (newpoint,) # rospy.loginfo("Sending trajectory:\n" + str(newtraj_arm)) rospy.loginfo("Publishing the arm trajectory") self.traj_publisher.publish(newtraj_arm)
def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict): if self._mode == "velocity": velocities = [0.0] * len(joint_names) cmd = dict(zip(joint_names, velocities)) while not self._server.is_new_goal_available() and self._alive: self._limb.set_joint_velocities(cmd) if self._cuff_state: self._limb.exit_control_mode() break rospy.sleep(1.0 / self._control_rate) elif self._mode == "position" or self._mode == "position_w_id": raw_pos_mode = self._mode == "position_w_id" if raw_pos_mode: pnt = JointTrajectoryPoint() pnt.positions = self._get_current_position(joint_names) if dimensions_dict["velocities"]: pnt.velocities = [0.0] * len(joint_names) if dimensions_dict["accelerations"]: pnt.accelerations = [0.0] * len(joint_names) while not self._server.is_new_goal_available() and self._alive: self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode) # zero inverse dynamics feedforward command if self._mode == "position_w_id": pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time) ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt) self._pub_ff_cmd.publish(ff_pnt) if self._cuff_state: self._limb.exit_control_mode() break rospy.sleep(1.0 / self._control_rate)
def make_trajectory_msg(plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id="/base_link"): """ This function will convert the plan to a joint trajectory compatible with the /arm_N/arm_controller/command topic """ theplan = plan # create joint trajectory message jt = JointTrajectory() # fill the header jt.header.seq = seq jt.header.stamp.secs = 0 # secs jt.header.stamp.nsecs = 0 # nsecs jt.header.frame_id = frame_id # specify the joint names jt.joint_names = theplan.joint_trajectory.joint_names # fill the trajectory points and computer constraint times njtp = len(theplan.joint_trajectory.points) for ii in range(0, njtp): jtp = JointTrajectoryPoint() jtp = copy.deepcopy(theplan.joint_trajectory.points[ii]) jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0] jtp.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0] jtp.time_from_start = rospy.Duration.from_sec(secs + dt * (ii + 1)) jt.points.append(jtp) return jt
def move(self, angles): goal = JointTrajectoryGoal() char = self.name[0] #either 'r' or 'l' goal.trajectory.joint_names = [char+'_shoulder_pan_joint', char+'_shoulder_lift_joint', char+'_upper_arm_roll_joint', char+'_elbow_flex_joint', char+'_forearm_roll_joint', char+'_wrist_flex_joint', char+'_wrist_roll_joint'] point = JointTrajectoryPoint() new_angles = list(angles) #replace angles that have None with last commanded joint position from this Arm class. for i,ang in enumerate(angles): if ang is None: assert self.last_angles is not None new_angles[i] = self.last_angles[i] self.last_angles = new_angles point.positions = new_angles point.time_from_start = rospy.Duration(3) goal.trajectory.points.append(point) self.jta.send_goal_and_wait(goal)
def check_collision(self): if not self.collsion_srv: return False # Create a new trajectory by combining both the trajectories, assumption is that the timing is same # The order of joining should be [left + right] self.adjust_traj_length() traj = JointTrajectory() traj.joint_names = self.left._goal.trajectory.joint_names + self.right._goal.trajectory.joint_names no_points = len(self.left._goal.trajectory.points) for index in xrange(no_points): pt = JointTrajectoryPoint() left_pt = self.left._goal.trajectory.points[index] right_pt = self.right._goal.trajectory.points[index] pt.positions = left_pt.positions + right_pt.positions pt.velocities = left_pt.velocities + right_pt.velocities pt.time_from_start = left_pt.time_from_start traj.points.append(pt) msg = CollisionCheckRequest() msg.trajectory = traj try: collision_service = rospy.ServiceProxy("collision_check", CollisionCheck) resp = collision_service(msg) if resp.collision_found: rospy.logwarn("Collision Found") else: rospy.loginfo("Collision not found, good to go :)") return resp.collision_found except rospy.ServiceException as e: rospy.logwarn("Exception on collision check {}".format(e)) return True
def execute(self, userdata): rospy.loginfo('In create_move_group_joints_goal') _move_joint_goal = FollowJointTrajectoryGoal() _move_joint_goal.trajectory.joint_names = userdata.move_joint_list jointTrajectoryPoint = JointTrajectoryPoint() jointTrajectoryPoint.positions = userdata.move_joint_poses for x in range(0, len(userdata.move_joint_poses)): jointTrajectoryPoint.velocities.append(0.0) jointTrajectoryPoint.time_from_start = rospy.Duration(2.0) _move_joint_goal.trajectory.points.append(jointTrajectoryPoint) for joint in _move_joint_goal.trajectory.joint_names: goal_tol = JointTolerance() goal_tol.name = joint goal_tol.position = 5.0 goal_tol.velocity = 5.0 goal_tol.acceleration = 5.0 _move_joint_goal.goal_tolerance.append(goal_tol) _move_joint_goal.goal_time_tolerance = rospy.Duration(2.0) _move_joint_goal.trajectory.header.stamp = rospy.Time.now() rospy.loginfo('Move_Joint_GOAL: ' + str(_move_joint_goal)) userdata.move_joint_goal = _move_joint_goal return 'succeeded'
def set_jep(self, arm, q, duration=0.15): if q is None or len(q) != 7: raise RuntimeError("set_jep value is " + str(q)) self.arm_state_lock[arm].acquire() jtg = JointTrajectoryGoal() jtg.trajectory.joint_names = self.joint_names_list[arm] jtp = JointTrajectoryPoint() jtp.positions = q #jtp.velocities = [0 for i in range(len(q))] #jtp.accelerations = [0 for i in range(len(q))] jtp.time_from_start = rospy.Duration(duration) jtg.trajectory.points.append(jtp) self.joint_action_client[arm].send_goal(jtg) self.jep[arm] = q cep, r = self.arms.FK_all(arm, q) self.arm_state_lock[arm].release() o = np.matrix([0.,0.,0.,1.]).T cep_marker = hv.single_marker(cep, o, 'sphere', '/torso_lift_link', color=(0., 0., 1., 1.), scale = (0.02, 0.02, 0.02), m_id = self.cep_marker_id) cep_marker.header.stamp = rospy.Time.now() self.marker_pub.publish(cep_marker)
def get_place_locations(self): locations = [] for xx in np.arange(0.4, 0.7, 0.1): for yy in np.arange(0.5, 0.9, 0.1): pl = PlaceLocation() pt = JointTrajectoryPoint() pt.positions = [0.5] pl.post_place_posture.points.append(pt) pl.post_place_posture.joint_names = ['l_gripper_motor_screw_joint'] pl.place_pose.header.frame_id = "odom_combined" pl.place_pose.header.stamp = rospy.Time.now() pl.place_pose.pose.position.x = xx pl.place_pose.pose.position.y = yy pl.place_pose.pose.position.z = 1.05 # Quaternion obtained by pointing wrist down #pl.place_pose.pose.orientation.w = 1.0 pl.place_pose.pose.orientation.x = 0.761 pl.place_pose.pose.orientation.y = -0.003 pl.place_pose.pose.orientation.z = -0.648 pl.place_pose.pose.orientation.w = 0.015 pl.pre_place_approach.direction.header.frame_id = "odom_combined" pl.pre_place_approach.direction.vector.z = -1.0 pl.pre_place_approach.desired_distance = 0.2 pl.pre_place_approach.min_distance = 0.1 pl.post_place_retreat.direction.header.frame_id = "odom_combined" pl.post_place_retreat.direction.vector.z = 1.0 pl.post_place_retreat.desired_distance = 0.2 pl.post_place_retreat.min_distance = 0.1 locations.append(pl) return locations
def _execute_trajectory(self, trajectory): if self._running: return self._running = True joint_ids = [self._current_joint_states.name.index(joint_name) for joint_name in trajectory.joint_names] position_previous = JointTrajectoryPoint() position_previous.positions = [self._current_joint_states.position[joint_id] for joint_id in joint_ids] # print 'starting from: ' + ('%6.2f ' * len(joint_ids)) % tuple(position_previous.positions) position_previous.time_from_start = rospy.Duration(0.0) position_index = 0 start_time = rospy.get_rostime() while position_index < len(trajectory.points): now = rospy.get_rostime() - start_time # print ' to: ' + ('%6.2f ' * len(joint_ids)) % tuple(trajectory.points[position_index].positions) while now <= trajectory.points[position_index].time_from_start: delta_time = now - position_previous.time_from_start position_duration = trajectory.points[position_index].time_from_start - position_previous.time_from_start if position_duration.to_sec() == 0.0: progress = 1.0 else: progress = delta_time.to_sec() / position_duration.to_sec() for joint_index, goal_position in enumerate(trajectory.points[position_index].positions): joint_delta = progress * (goal_position - position_previous.positions[joint_index]) joint_position = position_previous.positions[joint_index] + joint_delta self._joint_states.position[joint_index] = joint_position self._state_publisher.publish(self._joint_states) now = rospy.get_rostime() - start_time rospy.sleep(0.01) position_previous = trajectory.points[position_index] position_index += 1 self._running = False
def head_up_msg(self): jtm = JointTrajectory() jtm.joint_names = self.joint_names jtp = JointTrajectoryPoint() jtp.positions = [0]*len(self.joint_names) jtp.time_from_start = rospy.Duration(1.0) jtm.points = [jtp] return jtm
def send_goal(self, goal): """ Send goal to the controller. :param JointTrajectory goal: Goal to send. """ self.goal_done = False self.last_result = None self.last_feedback = None # Check which of our joints the goal has joints_in_goal = [] for j in goal.joint_names: if j in self.managed_joints: joints_in_goal.append(j) rospy.loginfo("This controller manages the joints: " + str(self.managed_joints)) rospy.loginfo("Goal contains our joints: " + str(joints_in_goal)) missing_joints = list(set(self.managed_joints) - set(joints_in_goal)) rospy.loginfo("Goal is missing joints: " + str(missing_joints)) extra_joints = list(set(goal.joint_names) - set(joints_in_goal)) rospy.loginfo("Discarding joints not from this controller: " + str(extra_joints)) if len(extra_joints) > 0: # Create a new goal only with the found joints, in the next # step it will be filled with any missing joints jt = JointTrajectory() jt.joint_names = joints_in_goal for jp in goal.points: p = JointTrajectoryPoint() p.time_from_start = jp.time_from_start for jname in joints_in_goal: idx = goal.joint_names.index(jname) p.positions.append(jp.positions[idx]) if jp.velocities: p.velocities.append(jp.velocities[idx]) if jp.accelerations: p.accelerations.append(jp.accelerations[idx]) if jp.effort: p.effort.append(jp.effort[idx]) jt.points.append(p) goal = jt if len(missing_joints) == len(self.managed_joints): # This goal contains no joints of this controller # we don't do anything rospy.loginfo("Goal contains no joints of this controller.") return if len(missing_joints) > 0: # Fill message for the real controller with the missing joints # Transform tuple fields into lists goal.joint_names = list(goal.joint_names) for point in goal.points: # type: JointTrajectoryPoint point.positions = list(point.positions) if point.velocities: point.velocities = list(point.velocities) if point.accelerations: point.accelerations = list(point.accelerations) if point.effort: point.effort = list(point.effort) # Now add the missing joints for jname in missing_joints: goal.joint_names.append(jname) jidx = self.managed_joints.index(jname) # TODO: remove this if-assert (it's redundant) if jidx == -1: rospy.logerror("Couldn't find joint " + jname + ", which we should be controlling," + " in our managed joints.") assert False for point in goal.points: point.positions.append( self.last_state.actual.positions[jidx]) if point.velocities: point.velocities.append( self.last_state.actual.velocities[jidx]) if self.last_state.actual.accelerations: point.accelerations.append( self.last_state.actual.accelerations[jidx]) # TODO: deal with forces, they aren't published in state... # but they are in joint_states # Now the trajectory should be complete and we can send the goal jtg = FollowJointTrajectoryGoal() jtg.trajectory = goal rospy.loginfo("Sending goal: " + str(goal)) self._ac.send_goal(jtg, done_cb=self.done_cb, feedback_cb=self.feedback_cb)
def getCartesianMove(self, frame, q0, base_steps=1000, steps_per_meter=1000, steps_per_radians=4, time_multiplier=1, percent_acc=1, use_joint_move=False, table_frame=None): if table_frame is not None: if frame.p[2] < table_frame[0][2]: rospy.logerr( "Ignoring move to waypoint due to relative z: %f < %f" % (frame.p[2], table_frame[0][2])) return JointTrajectory() if q0 is None: rospy.logerr("Invalid initial joint position in getCartesianMove") return JointTrajectory() # interpolate between start and goal pose = pm.fromMatrix(self.kdl_kin.forward(q0)) cur_rpy = np.array(pose.M.GetRPY()) cur_xyz = np.array(pose.p) goal_rpy = np.array(frame.M.GetRPY()) goal_xyz = np.array(frame.p) delta_rpy = np.linalg.norm(goal_rpy - cur_rpy) delta_translation = (pose.p - frame.p).Norm() if delta_rpy < 0.001 and delta_translation < 0.001: rospy.logwarn("Robot is already in the goal position.") return JointTrajectory(points=[ JointTrajectoryPoint(positions=q0, velocities=[0] * len(q0), accelerations=[0] * len(q0), time_from_start=rospy.Duration(0.0)) ], joint_names=self.joint_names) q_target = self.ik(pm.toMatrix(frame), q0) if q_target is None: rospy.logerr("No IK solution on cartesian move target") return JointTrajectory() else: if np.any( np.greater( np.absolute(q_target[:2] - np.array(q0[:2])), np.pi/2)) \ or np.absolute(q_target[3] - q0[3]) > np.pi: rospy.logerr("Dangerous IK solution, abort getCartesianMove") return JointTrajectory() dq_target = q_target - np.array(q0) if np.sum(np.absolute(dq_target)) < 0.0001: rospy.logwarn("Robot is already in the goal position.") return JointTrajectory(points=[ JointTrajectoryPoint(positions=q0, velocities=[0] * len(q0), accelerations=[0] * len(q0), time_from_start=rospy.Duration(0.0)) ], joint_names=self.joint_names) steps, t_v_const_step, t_v_setting_max, steps_to_max_speed, const_velocity_max_step = self.calculateAccelerationProfileParameters( dq_target, base_steps, steps_per_meter, steps_per_radians, delta_translation, time_multiplier, self.acceleration_magnification * percent_acc) traj = JointTrajectory() traj.points.append( JointTrajectoryPoint(positions=q0, velocities=[0] * len(q0), accelerations=[0] * len(q0))) # Compute a smooth trajectory. for i in range(1, steps + 1): xyz = None rpy = None q = None if not use_joint_move: xyz = cur_xyz + ((float(i) / steps) * (goal_xyz - cur_xyz)) rpy = cur_rpy + ((float(i) / steps) * (goal_rpy - cur_rpy)) # Create transform for goal frame frame = pm.toMatrix( kdl.Frame(kdl.Rotation.RPY(rpy[0], rpy[1], rpy[2]), kdl.Vector(xyz[0], xyz[1], xyz[2]))) # Use current inverse kinematics solver with current position q = self.ik(frame, q0) else: q = np.array(q0) + (float(i) / steps) * dq_target q = q.tolist() #q = self.kdl_kin.inverse(frame,q0) if self.verbose: print "%d -- %s %s = %s" % (i, str(xyz), str(rpy), str(q)) if q is not None: total_time = self.calculateTimeOfTrajectoryStep( i, steps_to_max_speed, const_velocity_max_step, t_v_const_step, t_v_setting_max) # Compute the distance to the last point for each joint. We use this to compute our joint velocities. dq_i = np.array(q) - np.array(traj.points[-1].positions) if np.sum(np.abs(dq_i)) < self.skip_tol: rospy.logwarn( "Joint trajectory point %d is repeating previous trajectory point. " % i) continue traj.points[i - 1].velocities = (dq_i) / ( total_time - traj.points[i - 1].time_from_start.to_sec()) pt = JointTrajectoryPoint(positions=q, velocities=[0] * len(q), accelerations=[0] * len(q)) pt.time_from_start = rospy.Duration(total_time) # pt.time_from_start = rospy.Duration(i * ts) traj.points.append(pt) else: rospy.logwarn( "No IK solution on one of the trajectory point to cartesian move target" ) if len(traj.points) < base_steps: print rospy.logerr("Planning failure with " \ + str(len(traj.points)) \ + " / " + str(base_steps) \ + " points.") return JointTrajectory() traj.joint_names = self.joint_names return traj
def getJointMove(self, q_goal, q0, base_steps=1000, steps_per_meter=1000, steps_per_radians=4, time_multiplier=1, percent_acc=1, use_joint_move=False, table_frame=None): if q0 is None: rospy.logerr("Invalid initial joint position in getJointMove") return JointTrajectory() elif np.all(np.isclose(q0, q_goal, atol=0.0001)): rospy.logwarn("Robot is already in the goal position.") return JointTrajectory() if np.any(np.greater(np.absolute(q_goal[:2] - np.array(q0[:2])), np.pi/2)) \ or np.absolute(q_goal[3] - q0[3]) > np.pi: # TODO: these thresholds should not be set manually here. rospy.logerr("Dangerous IK solution, abort getJointMove") return JointTrajectory() delta_q = np.array(q_goal) - np.array(q0) # steps = base_steps + int(np.sum(np.absolute(delta_q)) * steps_per_radians) steps, t_v_const_step, t_v_setting_max, steps_to_max_speed, const_velocity_max_step = self.calculateAccelerationProfileParameters( delta_q, base_steps, 0, steps_per_radians, 0, time_multiplier, self.acceleration_magnification * percent_acc) traj = JointTrajectory() traj.points.append( JointTrajectoryPoint(positions=q0, velocities=[0] * len(q0), accelerations=[0] * len(q0))) # compute IK for i in range(1, steps + 1): xyz = None rpy = None q = None q = np.array(q0) + (float(i) / steps) * delta_q q = q.tolist() if self.verbose: print "%d -- %s %s = %s" % (i, str(xyz), str(rpy), str(q)) if q is not None: dq_i = np.array(q) - np.array(traj.points[i - 1].positions) if np.sum(dq_i) < 0.0001: rospy.logwarn( "Joint trajectory point %d is repeating previous trajectory point. " % i) # continue total_time = total_time = self.calculateTimeOfTrajectoryStep( i, steps_to_max_speed, const_velocity_max_step, t_v_const_step, t_v_setting_max) traj.points[i - 1].velocities = dq_i / ( total_time - traj.points[i - 1].time_from_start.to_sec()) pt = JointTrajectoryPoint(positions=q, velocities=[0] * len(q), accelerations=[0] * len(q)) pt.time_from_start = rospy.Duration(total_time) traj.points.append(pt) else: rospy.logwarn( "No IK solution on one of the trajectory point to cartesian move target" ) if len(traj.points) < base_steps: print rospy.logerr("Planning failure with " \ + str(len(traj.points)) \ + " / " + str(base_steps) \ + " points.") return JointTrajectory() traj.joint_names = self.joint_names return traj
'right_rh_p12_rn', 'right_rh_r2', 'right_rh_l1', 'right_rh_l2' ] # Connect to the right arm trajectory action server print "Connecting to the right gripper trajectory action server..." gripper_client = actionlib.SimpleActionClient( '/dual_ur5_arm/right_gripper_controller/follow_joint_trajectory', FollowJointTrajectoryAction) gripper_client.wait_for_server() print "Connected" # Create an arm trajectory with the arm_goal as the end-point print "Setting up a goal trajectory..." gripper_trajectory = JointTrajectory() gripper_trajectory.joint_names = gripper_joints gripper_trajectory.points.append(JointTrajectoryPoint()) ang = 0.2 # upper is 1.1 # offset from ground is 0.142 when ang=0.72 gripper_trajectory.points[0].positions = [ang, ang / 1.1, ang, ang / 1.1] gripper_trajectory.points[0].velocities = [0.0 for i in gripper_joints] gripper_trajectory.points[0].accelerations = [0.0 for i in gripper_joints] gripper_trajectory.points[0].time_from_start = rospy.Duration(0.5) # Create an empty trajectory goal gripper_goal = FollowJointTrajectoryGoal() # set the trajectory component to the goal trajectory gripper_goal.trajectory = gripper_trajectory # specify zero tolerance for the execution time gripper_goal.goal_time_tolerance = rospy.Duration(0.0)
def close_gripper(self): self.gripper_msg.points = [JointTrajectoryPoint(positions=[0.274], velocities=[0], time_from_start=rospy.Duration(0.1))] self.griper_pos.publish(self.gripper_msg)
def executeTraj(trajdata): #torsogoal = SingleJointPositionGoal() torso_goal = JointTrajectoryGoal() arm_goal = JointTrajectoryGoal() # parse trajectory data into the ROS structure tokens = trajdata.split() numpoints = int(tokens[0]) dof = int(tokens[1]) trajoptions = int(tokens[2]) numvalues = dof offset = 0 if trajoptions & 4: # timestamps numvalues += 1 offset += 1 if trajoptions & 8: # base transforms numvalues += 7 if trajoptions & 16: # velocities numvalues += dof if trajoptions & 32: # torques numvalues += dof for i in range(numpoints): start = 3 + numvalues * i torso_pt = JointTrajectoryPoint() arm_pt = JointTrajectoryPoint() k = 0 for dofInd in armDofIndices: pos = float(tokens[start + offset + dofInd]) if jointNameByIndex[dofInd] == 'torso_lift_joint': if i > 0: prevpos = torso_goal.trajectory.points[-1].positions[0] newpos = prevpos + angleDistance(pos, prevpos) torso_pt.positions.append(newpos) else: torso_pt.positions.append(pos) else: if i > 0: prevpos = arm_goal.trajectory.points[-1].positions[k] newpos = prevpos + angleDistance(pos, prevpos) arm_pt.positions.append(newpos) else: arm_pt.positions.append(pos) k = k + 1 if trajoptions & 4: arm_pt.time_from_start = rospy.Duration(float(tokens[start])) torso_pt.time_from_start = rospy.Duration(float(tokens[start])) arm_goal.trajectory.joint_names = \ filter(lambda jn: jn!='torso_lift_joint', [jointNameByIndex[i] for i in armDofIndices]) arm_goal.trajectory.points.append(arm_pt) torso_goal.trajectory.joint_names = ['torso_lift_joint'] torso_goal.trajectory.points.append(torso_pt) print 'sending arm JointTrajectoryGoal' arm_joint_action_client.send_goal(arm_goal) print 'sending torso JointTrajectoryGoal' torso_joint_action_client.send_goal(torso_goal) arm_joint_action_client.wait_for_result() torso_joint_action_client.wait_for_result() arm_state = arm_joint_action_client.get_state() torso_state = torso_joint_action_client.get_state() return arm_state, torso_state
rospy.loginfo("Turn on motors") rospy.wait_for_service('/ve026a/set_motor') try: res = rospy.ServiceProxy('/ve026a/set_motor', SetBool)(True) print(res.message) except rospy.ServiceException, e: print("Service call failed, %s" % e) msg = JointTrajectory() msg.header.stamp = rospy.Time.now() msg.joint_names = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ] p1 = JointTrajectoryPoint() p1.positions = [0, 0, 0, 0, 0, 0] p1.velocities = [0, 0, 0, 0, 0, 0] p1.time_from_start = rospy.Duration.from_sec(3.0) p2 = JointTrajectoryPoint() p2.positions = [0.5, 0.5, 0, 0, 0, 0] p2.velocities = [0, 0, 0, 0, 0, 0] p2.time_from_start = rospy.Duration.from_sec(5.0) p3 = JointTrajectoryPoint() p3.positions = [0, 0, 0, 0, 0, 0] p3.velocities = [0, 0, 0, 0, 0, 0] p3.time_from_start = rospy.Duration.from_sec(7.0) msg.points = [p1, p2, p3]
def handle_calculate_IK(req): rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses)) if len(req.poses) < 1: print "No valid poses received" return -1 else: start_time = time() ### Forward Kinematics code # Create symbols #Joint Angle Symbols q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') #Link Offset Symbols d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') #Link Length symbols a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') #Twist Angle symbols alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols( 'alpha0:7') # Modified DH parameters DH_Table = { alpha0: 0, a0: 0, d1: 0.75, q1: q1, alpha1: -pi / 2., a1: 0.35, d2: 0, q2: -pi / 2. + q2, alpha2: 0, a2: 1.25, d3: 0, q3: q3, alpha3: -pi / 2., a3: -0.054, d4: 1.5, q4: q4, alpha4: pi / 2., a4: 0, d5: 0, q5: q5, alpha5: -pi / 2., a5: 0, d6: 0, q6: q6, alpha6: 0, a6: 0, d7: 0.303, q7: 0 } # Define Modified DH Transformation matrix def TF_Matrix(alpha, a, d, q): TF = Matrix([[cos(q), -sin(q), 0, a], [ sin(q) * cos(alpha), cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d ], [ sin(q) * sin(alpha), cos(q) * sin(alpha), cos(alpha), cos(alpha) * d ], [0, 0, 0, 1]]) return TF # Create individual transformation matrices T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(DH_Table) T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(DH_Table) T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(DH_Table) T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(DH_Table) T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(DH_Table) T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(DH_Table) T6_EE = TF_Matrix(alpha6, a6, d7, q7).subs(DH_Table) T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE # Extract rotation matrices from the transformation matrices R0_3_Temp = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3] # Define Roatation Matrix from End Effector Yaw, Roll, Pitch r, p, y = symbols('r p y') ROT_x = Matrix([ [1, 0, 0], #Roll [0, cos(r), -sin(r)], [0, sin(r), cos(r)] ]) ROT_y = Matrix([ [cos(p), 0, sin(p)], #Pitch [0, 1, 0], [-sin(p), 0, cos(p)] ]) ROT_z = Matrix([ [cos(y), -sin(y), 0], #Yaw [sin(y), cos(y), 0], [0, 0, 1] ]) ROT_EE = ROT_z * ROT_y * ROT_x ## Initializtion # Define Joint angles for Target Spawn locations for the END trajectory point #(theta1i,theta3i,theta4i,theta5i,theta6i, theta7i, theta8i, theta9i) are the joint angles for target sawn locations 1 to 9 # where i ranges from 1 to 6, while moving from initial location to target spawn location ) theta1 = 0 theta2 = 0 theta3 = 0 theta4 = 0 theta5 = 0 theta6 = 0 theta61 = -0.460788639682829 theta62 = -1.4083946795092 + pi / 2 theta63 = -1.49141431818708 + pi / 2 theta64 = -1.0235104903567 + pi theta65 = 0.495695716305110 theta66 = -pi + 0.972495930022601 theta41 = 0.466568380940290 theta42 = -1.40588840950334 + pi / 2 theta43 = -1.49933810543832 + pi / 2 theta44 = -pi + 1.12482883075263 theta45 = 0.521928836050086 theta46 = -1.06984849582538 + pi theta91 = -0.460620971603582 theta92 = -1.41600678927374 + pi / 2 theta93 = -2.01520122842406 + pi / 2 theta94 = 0.912819391695163 theta95 = 0.538148746753259 theta96 = -0.855105912394959 theta71 = 0.460651236883725 theta72 = -1.41603690529533 + pi / 2 theta73 = -2.015158587665 + pi / 2 theta74 = -0.913449451374226 theta75 = 0.537900753574318 theta76 = 0.855694311433023 theta81 = -4.02985219558988e-6 theta82 = -1.59017523803714 + pi / 2 theta83 = -1.81801235853875 + pi / 2 theta84 = -8.71196937061172e-5 theta85 = 0.259284638790722 theta86 = -7.35675934196834e-6 theta11 = 0.461944532159549 theta12 = -1.11836645228303 + pi / 2 theta13 = -1.31463177224073 + pi / 2 theta14 = -pi + 0.725225037468592 theta15 = 0.676095580016169 theta16 = -0.62726276643435 + pi theta51 = -0.00229054389158514 theta52 = -1.57765784191287 + pi / 2 theta53 = -1.32851692115255 + pi / 2 theta54 = -0.086229482068646 + pi theta55 = 0.181470117894438 theta56 = -pi + 0.488456526793796 theta31 = -0.466518984306794 theta32 = -1.08486429666301 + pi / 2 theta33 = -1.31281014892211 + pi / 2 theta34 = -0.618700551351137 + pi theta35 = 0.886705067213612 theta36 = -pi + 0.441222764426423 # Define Joint angle for the end trajectory point # (thetaei) is the joint angle while moving from target spawn location to bin) thetae1 = -1.3683474710526 + pi thetae2 = -0.965048732979534 + pi / 2 thetae3 = -2.09403813236587 + pi / 2 thetae4 = -1.55313297331450 thetae5 = -1.3697836855635 + pi thetae6 = -1.48647561055299 + pi # Initialize service response ReachCount = 0 joint_trajectory_list = [] ee_offset_list = [] # Create List of error in end effector time_list = [] # Create list of time samples if (len(req.poses) % 2) == 0: # For loop initiation of req.poses i = 1 else: i = 0 if (len(req.poses) > 50): # For loop initiation of req.poses j = 2 else: j = 1 i = 0 Count.count = Count.count + 1 # Incrementing Iteration count for number of Inverse Kinematics loop print("IK count: %s" % (Count.count)) # Calcukating Target Spawn END locations on shelf from request responses goal_px = req.poses[len(req.poses) - 1].position.x goal_py = req.poses[len(req.poses) - 1].position.y goal_pz = req.poses[len(req.poses) - 1].position.z print("\nGoal position along x axis: %04.8f" % goal_px) print("Goal position along y axis: %04.8f" % goal_py) print("Goal position along z axis: %04.8f" % goal_pz) Target_Spawn = 0 if ((Count.count % 2) == 1): if (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= -1) & ( goal_py < -0.8) & (goal_pz >= 1.58) & ( goal_pz <= 1.59): #Target Spawn location: 6 on shelf Target_Spawn = 6 elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= 0.8) & ( goal_py < 1) & (goal_pz >= 2.3) & ( goal_pz <= 2.5): #Target Spawn location: 7 on shelf Target_Spawn = 7 elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= 0.8) & ( goal_py < 1) & (goal_pz >= 1.58) & ( goal_pz <= 1.59): #Target Spawn location: 4 on shelf Target_Spawn = 4 elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= -1) & ( goal_py < -0.8) & (goal_pz >= 2.3) & ( goal_pz <= 2.5): #Target Spawn location: 9 on shelf Target_Spawn = 9 elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= -0.1) & ( goal_py < 0.1) & (goal_pz >= 2.3) & ( goal_pz <= 2.5): #Target Spawn location: 8 on shelf Target_Spawn = 8 elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= 0.8) & ( goal_py < 1) & (goal_pz >= 0.7) & ( goal_pz <= 0.9): #Target Spawn location: 1 on shelf Target_Spawn = 1 elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= -1) & ( goal_py < -0.8) & (goal_pz >= 0.7) & ( goal_pz <= 0.9): #Target Spawn location: 3 on shelf Target_Spawn = 3 elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= -0.1) & ( goal_py < 0.1) & (goal_pz >= 1.58) & ( goal_pz <= 1.59): #Target Spawn location: 5 on shelf Target_Spawn = 5 print("\nTarget spawn location is: %s" % (Target_Spawn)) for x in xrange(i, len(req.poses), j): joint_trajectory_point = JointTrajectoryPoint() print(x) # Extract end-effector position and orientation from request # px,py,pz = end-effector position # roll, pitch, yaw = end-effector orientation px = req.poses[x].position.x py = req.poses[x].position.y pz = req.poses[x].position.z (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([ req.poses[x].orientation.x, req.poses[x].orientation.y, req.poses[x].orientation.z, req.poses[x].orientation.w ]) # Compensate for rotation discrepancy between DH parameters and Gazebo Rot_Error = ROT_z.subs(y, radians(180)) * ROT_y.subs( p, radians(-90)) ROT_EE = ROT_EE * Rot_Error ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw}) # Calculate End Effetor position EE = Matrix([[px], [py], [pz]]) # Wrist Center Calculation WC = EE - (0.303) * ROT_EE[:, 2] ### Inverse Kinematics code if len(joint_trajectory_list ) > 2: #Calculate previous joint angles z = (joint_trajectory_list[len(joint_trajectory_list) - 1]) theta1 = z.positions[0] theta2 = z.positions[1] theta3 = z.positions[2] theta4 = z.positions[3] theta5 = z.positions[4] theta6 = z.positions[5] if (len(joint_trajectory_list) > 2) & ( (Count.count % 2) == 0) & (x < (len(req.poses) - 1)) & ( theta1 > -1.62 + pi) & (theta1 < -1.36 + pi) & ( theta2 > -1.1 + pi / 2) & (theta2 < -0.94 + pi / 2) & ( theta3 > -2.2 + pi / 2) & (theta3 < -1.9 + pi / 2): print('Reached TargetLocation' ) # Kuka arm has reached near the bin if (ReachCount == 0): ReachCount = 1 joint_trajectory_point.positions = [ thetae1, thetae2, thetae3, thetae4, thetae5, thetae6 ] joint_trajectory_list.append(joint_trajectory_point) else: # Calculate joint angles using Geometric IK method theta1 = atan2(WC[1], WC[0]) side_b = sqrt( pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) + pow( (WC[2] - 0.75), 2)) #side_a = 1.501 #side_c = 1.25 if ((1.25 + 1.501) > side_b) & ((1.25 + side_b) > 1.501) & ( (side_b + 1.501) > 1.25 ): # Mathematically, (sum of 2 sides of a triangle) > 3rd side angle_a = acos( (side_b * side_b + 1.25 * 1.25 - 1.501 * 1.501) / (2 * side_b * 1.25)) angle_b = acos( (1.501 * 1.501 + 1.25 * 1.25 - side_b * side_b) / (2 * 1.501 * 1.25)) angle_c = acos( (1.501 * 1.501 + side_b * side_b - 1.25 * 1.25) / (2 * 1.501 * side_b)) theta2 = pi / 2 - angle_a - atan2( WC[2] - 0.75, sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35) theta3 = pi / 2 - (angle_b + atan2(0.054, 1.5)) R0_3 = R0_3_Temp.evalf(subs={ q1: theta1, q2: theta2, q3: theta3 }) # Evaluating Rotation Matrix R3_6 = R0_3.inv("LU") * ROT_EE theta4 = atan2(R3_6[2, 2], -R3_6[0, 2]) theta5 = atan2( sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]), R3_6[1, 2]) theta6 = atan2(-R3_6[1, 1], R3_6[1, 0]) # Forward Kinematics evaluation with derived joint angles FK = T0_EE.evalf( subs={ q1: theta1, q2: theta2, q3: theta3, q4: theta4, q5: theta5, q6: theta6 }) # Error Calculation of End Effector position by comparing with end-effector poses, received as input ee_x_e = abs(FK[0, 3] - px) # End Effector error offset along x axis ee_y_e = abs(FK[1, 3] - py) # End Effector error offset along y axis ee_z_e = abs(FK[2, 3] - pz) # End Effector error offset along z axis # Overall end-effector offset ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2) print("\nEnd effector error for x position is: %04.8f" % ee_x_e) print("End effector error for y position is: %04.8f" % ee_y_e) print("End effector error for z position is: %04.8f" % ee_z_e) print("Overall end effector offset is: %04.8f units \n" % ee_offset) # Populate response for the IK request if (x == len(req.poses) - 1) & ( Target_Spawn == 6 ): #Last joint angle calculation for Target Spawn location: 6 if ((Count.count % 2) == 1): joint_trajectory_point.positions = [ theta61, theta62, theta63, theta64, theta65, theta66 ] joint_trajectory_list.append( joint_trajectory_point) elif (x == len(req.poses) - 1) & ( Target_Spawn == 4 ): #Last joint angle calculation for Target Spawn location: 4 if ((Count.count % 2) == 1): joint_trajectory_point.positions = [ theta41, theta42, theta43, theta44, theta45, theta46 ] joint_trajectory_list.append( joint_trajectory_point) elif (x == len(req.poses) - 1) & ( Target_Spawn == 9 ): #Last joint angle calculation for Target Spawn location: 9 if ((Count.count % 2) == 1): joint_trajectory_point.positions = [ theta91, theta92, theta93, theta94, theta95, theta96 ] joint_trajectory_list.append( joint_trajectory_point) elif (x == len(req.poses) - 1) & ( Target_Spawn == 7 ): #Last joint angle calculation for Target Spawn location: 7 if ((Count.count % 2) == 1): joint_trajectory_point.positions = [ theta71, theta72, theta73, theta74, theta75, theta76 ] joint_trajectory_list.append( joint_trajectory_point) elif (x == len(req.poses) - 1) & ( Target_Spawn == 8 ): #Last joint angle calculation for Target Spawn location: 8 if ((Count.count % 2) == 1): joint_trajectory_point.positions = [ theta81, theta82, theta83, theta84, theta85, theta86 ] joint_trajectory_list.append( joint_trajectory_point) elif (x == len(req.poses) - 1) & ( Target_Spawn == 1 ): #Last joint angle calculation for Target Spawn location: 1 if ((Count.count % 2) == 1): joint_trajectory_point.positions = [ theta11, theta12, theta13, theta14, theta15, theta16 ] joint_trajectory_list.append( joint_trajectory_point) elif (x == len(req.poses) - 1) & ( Target_Spawn == 5 ): #Last joint angle calculation for Target Spawn location: 5 if ((Count.count % 2) == 1): joint_trajectory_point.positions = [ theta51, theta52, theta53, theta54, theta55, theta56 ] joint_trajectory_list.append( joint_trajectory_point) elif (x == len(req.poses) - 1) & ( Target_Spawn == 3 ): #Last joint angle calculation for Target Spawn location: 3 if ((Count.count % 2) == 1): joint_trajectory_point.positions = [ theta31, theta32, theta33, theta34, theta35, theta36 ] joint_trajectory_list.append( joint_trajectory_point) elif (x == len(req.poses) - 1) & ( (Count.count % 2) == 0): #Last joint angle calculation towards Bin if (ReachCount == 0): if (ee_offset < 0.01): joint_trajectory_point.positions = [ theta1, theta2, theta3, thetae4, thetae5, thetae6 ] joint_trajectory_list.append( joint_trajectory_point) else: joint_trajectory_point.positions = [ thetae1, thetae2, thetae3, thetae4, thetae5, thetae6 ] joint_trajectory_list.append( joint_trajectory_point) zt = (joint_trajectory_list[ len(joint_trajectory_list) - 2]) Theta_6 = zt.positions[5] Theta6Diff = (np.absolute(Theta_6 - thetae6)) if (Theta6Diff >= 1.7): joint_trajectory_point.positions = [ thetae1, thetae2, thetae3, thetae4, thetae5, Theta_6 ] joint_trajectory_list.append( joint_trajectory_point) elif ( ee_offset < 0.01 ): #Allow joint angles for which error is below threshold joint_trajectory_point.positions = [ theta1, theta2, theta3, theta4, theta5, theta6 ] joint_trajectory_list.append(joint_trajectory_point) ee_offset_list.append(ee_offset) t = time() time_list.append(t) print( "\nTotal run time to calculate joint angles from pose is %04.4f seconds" % (time() - start_time)) rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list)) ## Plotting error outputs of end effector w.r.t to received responses (Close the plot window for code to run further) ## UNCOMMENT BELOW LINES TO CHECK THE ERROR PLOTS FOR CHALLENGE COMPLETION #ee_offset_ar = np.array([ee_offset_list]) #time_list_ar = np.array([time_list]) #plt.figure(figsize=(15,7)) #plt.rc('font', family='serif', size=15) #plt.plot(time_list_ar,ee_offset_ar, marker='o', color='r') #plt.xlabel('Time',fontsize=18) #plt.ylabel('End Effector Offset',fontsize=18) #plt.grid(True) #plt.show() return CalculateIKResponse(joint_trajectory_list)
def main(whole_body, base, gripper, wrist_wrench): cli = actionlib.SimpleActionClient( '/hsrb/omni_base_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction) # publisher for delvering command for base move vel_pub = rospy.Publisher('/hsrb/command_velocity', geometry_msgs.msg.Twist, queue_size=10) # base_pub = rospy.Publisher('/move_base/move/goal',move_base_msgs.msg.MoveBaseActionGoal,queue_size=10) armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command', JointTrajectory, queue_size=1) ## Grab the handle of door #test with service - get the handle position from handle grasp_point_client() global recog_pos global Is_found print recog_pos.pose.position # target_pose_Msg = rospy.wait_for_message("/handle_detector/grasp_point", PoseStamped) # recog_pos.pose.position.x=target_pose_Msg.pose.position.x # recog_pos.pose.position.y=target_pose_Msg.pose.position.y # recog_pos.pose.position.z=target_pose_Msg.pose.position.z whole_body.move_to_neutral() # whole_body.impedance_config= 'grasping' switch = ImpedanceControlSwitch() # wrist_wrench.reset() gripper.command(1.0) grab_pose = geometry.multiply_tuples( geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_, y=recog_pos.pose.position.y - 0.012, z=recog_pos.pose.position.z, ej=math.pi / 2), geometry.pose(ek=math.pi / 2)) whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF) wrist_wrench.reset() # whole_body.impedance_config= 'compliance_middle' switch.activate("grasping") # gripper.command(0.01) gripper.grasp(-0.01) rospy.sleep(1.0) switch.inactivate() wrist_wrench.reset() rospy.sleep(8.0) #### test manipulation whole_body.impedance_config = 'grasping' # print(whole_body.impedance_config) # desired_rot=-1.95 # whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot}) traj = JointTrajectory() # This controller requires that all joints have values traj.joint_names = [ "arm_lift_joint", "arm_flex_joint", "arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] p = JointTrajectoryPoint() current_positions = [latest_positions[name] for name in traj.joint_names] current_positions[0] = latest_positions["arm_lift_joint"] - 0.07 current_positions[1] = latest_positions["arm_flex_joint"] - 0.02 current_positions[2] = latest_positions["arm_roll_joint"] current_positions[3] = latest_positions["wrist_flex_joint"] current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65 p.positions = current_positions p.velocities = [0, 0, 0, 0, 0] p.time_from_start = rospy.Time(3) traj.points = [p] armPub.publish(traj) rospy.sleep(3.0) # goal_pose =PoseStamped() # goal_pose.pose.position.x=recog_pos.pose.position.x+0.4 # goal_pose.pose.position.y=recog_pos.pose.position.y+0.2 # goal_pose.pose.position.z=recog_pos.pose.position.z # goal_pose.pose.orientation.x=recog_pos.pose.orientation.x # goal_pose.pose.orientation.y=recog_pos.pose.orientation.y # goal_pose.pose.orientation.z=recog_pos.pose.orientation.z # goal_pose.pose.orientation.w=recog_pos.pose.orientation.w # yaw=math.pi/6 # quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw) # #type(pose) = geometry_msgs.msg.Pose # nav_goal = move_base_msgs.msg.MoveBaseActionGoal() # nav_goal.header.stamp=rospy.Time(0) # nav_goal.goal.target_pose.header.frame_id = "map" # nav_goal.goal.target_pose.pose.position.x=recog_pos.pose.position.x+0.3 # nav_goal.goal.target_pose.pose.position.y=recog_pos.pose.position.y+0.2 # nav_goal.goal.target_pose.pose.position.z=0.0 # nav_goal.goal.target_pose.pose.orientation.x=quaternion[0] # nav_goal.goal.target_pose.pose.orientation.y=quaternion[1] # nav_goal.goal.target_pose.pose.orientation.z=quaternion[2] # nav_goal.goal.target_pose.pose.orientation.w=quaternion[3] # base_pub.publish(nav_goal) # # whole_body.end_effector_frame = u'odom' # # whole_body.move_end_effector_by_line((0, 0, 1), -0.2) # # publish_arm(latest_positions["arm_lift_joint"],latest_positions["arm_flex_joint"],latest_positions["arm_roll_joint"], latest_positions["wrist_flex_joint"],wrist_roll) whole_body.impedance_config = 'compliance_hard' whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45) # # whole_body.move_end_effector_by_line((0.7071, 0.7071,0), 0.5) # # whole_body.move_end_effector_by_line((0, -0.7071, 0.7071), 0.5) # whole_body.impedance_config= None tw = geometry_msgs.msg.Twist() tw.linear.x = 0.9 tw.angular.z = 0.45 vel_pub.publish(tw) rospy.sleep(4.0) tw_cmd0 = geometry_msgs.msg.Twist() tw_cmd0.linear.x = 0.3 tw_cmd0.angular.z = 0.45 vel_pub.publish(tw_cmd0) # rospy.sleep(4.0) rospy.sleep(4.0) gripper.command(1.0) # whole_body.move_end_effector_by_line((0, 0, -1), 0.25) tw_cmd = geometry_msgs.msg.Twist() tw_cmd.linear.x = -1.2 vel_pub.publish(tw_cmd) rospy.sleep(2.0) tw_cmd2 = geometry_msgs.msg.Twist() tw_cmd2.linear.x = -1.1 tw_cmd2.angular.z = -0.4 vel_pub.publish(tw_cmd2) rospy.sleep(4.0) whole_body.move_to_neutral() tw_cmd2 = geometry_msgs.msg.Twist() tw_cmd2.linear.x = -0.6 tw_cmd2.angular.z = -0.3 vel_pub.publish(tw_cmd2)
def handle_calculate_IK(req): rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses)) if len(req.poses) < 1: print "No valid poses received" return -1 else: ### Your FK code here # Create symbols q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols( 'alpha0:7') # Create Modified DH parameters s = { alpha0: 0, a0: 0, d1: 0.75, q1: q1, alpha1: -pi / 2, a1: 0.35, d2: 0, q2: q2 - pi / 2, alpha2: 0, a2: 1.25, d3: 0, q3: q3, alpha3: -pi / 2, a3: -0.054, d4: 1.50, q4: q4, alpha4: pi / 2, a4: 0, d5: 0, q5: q5, alpha5: -pi / 2, a5: 0, d6: 0, q6: q6, alpha6: 0, a6: 0, d7: 0.303, q7: 0 } # Define Modified DH Transformation matrix def DHTmatrix(q, d, a, alpha): DHTmatrix = Matrix([[cos(q), -sin(q), 0, a], [ sin(q) * cos(alpha), cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d ], [ sin(q) * sin(alpha), cos(q) * sin(alpha), cos(alpha), cos(alpha) * d ], [0, 0, 0, 1]]) return DHTmatrix # Create individual transformation matrices T0_1 = DHTmatrix(q1, d1, a0, alpha0).subs(s) T1_2 = DHTmatrix(q2, d2, a1, alpha1).subs(s) T2_3 = DHTmatrix(q3, d3, a2, alpha2).subs(s) T3_4 = DHTmatrix(q4, d4, a3, alpha3).subs(s) T4_5 = DHTmatrix(q5, d5, a4, alpha4).subs(s) T5_6 = DHTmatrix(q6, d6, a5, alpha5).subs(s) T6_7 = DHTmatrix(q7, d7, a6, alpha6).subs(s) T0_7 = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_7 # Initialize service response joint_trajectory_list = [] for x in xrange(0, len(req.poses)): # IK code starts here joint_trajectory_point = JointTrajectoryPoint() # Extract end-effector position and orientation from request # px,py,pz = end-effector position # roll, pitch, yaw = end-effector orientation px = req.poses[x].position.x py = req.poses[x].position.y pz = req.poses[x].position.z (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([ req.poses[x].orientation.x, req.poses[x].orientation.y, req.poses[x].orientation.z, req.poses[x].orientation.w ]) r, p, y = symbols('r p y') # Define Roll Matrix about x-axis R_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0, sin(r), cos(r)]]) # Define Pitch Matrix about y-axis R_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0], [-sin(p), 0, cos(p)]]) # Define Yaw Matrix about z-axis R_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0, 1]]) # Compensate for rotation discrepancy between DH parameters and Gazebo R_corr = R_z.subs(y, pi) * R_y.subs(p, -pi / 2) Rrpy = R_x.subs(r, roll) * R_y.subs(p, pitch) * R_z.subs( y, yaw) * R_corr # Calculate Wrist Center position EE = Matrix([[px], [py], [pz]]) WC = EE - (0.303) * Rrpy[:, 2] # Calculate joint angles using Geometric IK method theta1 = atan2(WC[1], WC[0]) # Calculate Sides of Triangle Formed by Origin 2, Origin 3, and the WC Origin side_a = 1.5009716852759081944053710341178 side_b = sqrt((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)**2 + (WC[2] - 0.75)**2) side_c = 1.25 # Calculate Angles of Triangle Formed by Origin 2, Origin 3, and the WC Origin angle_a = acos( (side_b * side_b + side_c * side_c - side_a * side_a) / (2 * side_b * side_c)) angle_b = acos( (side_a * side_a + side_c * side_c - side_b * side_b) / (2 * side_a * side_c)) angle_c = acos( (side_a * side_a + side_b * side_b - side_c * side_c) / (2 * side_a * side_b)) # Calculate theta2 and theta3 theta2 = pi / 2 - angle_a - atan2(WC[2] - 0.75, sqrt(WC[0]**2 + WC[1]**2) - 0.35) theta3 = pi / 2 - (angle_b + 0.036) # Calculate theta4, theta5, and theta6 R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3] R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3}) R3_6 = R0_3.inv("LU") * Rrpy theta4 = atan2(R3_6[2, 2], -R3_6[0, 2]) theta5 = atan2( sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]), R3_6[1, 2]) theta6 = atan2(-R3_6[1, 1], R3_6[1, 0]) # Populate response for the IK request # In the next line replace theta1,theta2...,theta6 by your joint angle variables joint_trajectory_point.positions = [ theta1, theta2, theta3, theta4, theta5, theta6 ] joint_trajectory_list.append(joint_trajectory_point) rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list)) return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req): rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses)) if len(req.poses) < 1: print "No valid poses received" return -1 else: ### # Initialize service response print("Total poses: " + str(len(req.poses))) joint_trajectory_list = [] for x in xrange(0, len(req.poses)): # IK code starts here joint_trajectory_point = JointTrajectoryPoint() # Extract end-effector position and orientation from request # px,py,pz = end-effector position # roll, pitch, yaw = end-effector orientation px = req.poses[x].position.x py = req.poses[x].position.y pz = req.poses[x].position.z (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([ req.poses[x].orientation.x, req.poses[x].orientation.y, req.poses[x].orientation.z, req.poses[x].orientation.w ]) pwc_ee = Matrix([px, py, pz]) ### Your IK code here # R_rpy_eval = R_rpy.evalf(subs={r: roll, p: pitch, y: yaw}) R_rpy_eval = R_rpy.subs({r: roll, p: pitch, y: yaw}) # Get the wrist-center p0_wc = pwc_ee - ( s[d7]) * R_rpy_eval[:, 2] # wc in terms of the base link # Calculate joint angles using Geometric IK method # # # Using law of cosines to get theta 1, 2 and 3 # set the length of the sides of the triangle joint2, 3 and 4 side_a = s[d4] side_b = sqrt( pow((sqrt(p0_wc[0]**2 + p0_wc[1]**2) - s[a1]), 2) + pow( (p0_wc[2] - s[d1]), 2)) # i dont know why?! side_c = s[a2] # Get the angles angle_a = abs( acos((side_b**2 + side_c**2 - side_a**2) / (2 * side_b * side_c))) angle_b = abs( acos((side_a**2 + side_c**2 - side_b**2) / (2 * side_a * side_c))) # angle_c = acos((side_a**2 + side_b**2 - side_c**2)/(2*side_a*side_b)) # Get theta1, 2 and 3 # using pythagorean. Draw the kinematics out to understand theta1 = atan2(p0_wc[1], p0_wc[0]) # theta2 is a subset of angle_a, so we remove the angle that's not related to theta2 theta2 = pi / 2 - angle_a - atan2( p0_wc[2] - s[d1], sqrt(p0_wc[0]**2 + p0_wc[1]**2) - s[a1]) # theta3 is a subset of angle_b, so we remove the angle that is not theta3 sag_angle = atan2(0.054, sqrt(1.501**2 - 0.054**2)) theta3 = pi / 2 - (angle_b + sag_angle) # Get the rotation from joint 3 to the ee # evaluate the rotation from base link to link3 (joint 0 to joint 3) # because we now have the theta for these joints that defines the pose of the wrist center R0_3_eval = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3}) # The inverse of R0_3_eval yields R3_0, R_rpy_eval is # R3_6 = R0_3_eval.inv("LU") * R_rpy_eval R3_6 = R0_3_eval.transpose() * R_rpy_eval # We get theta4, 5, 6 from R3_G which now contains the evaluated # rotation from base link to wrist center theta5 = atan2(sqrt(R3_6[0, 2]**2 + R3_6[2, 2]**2), R3_6[1, 2]) # If sin(theta5) is negative, switch appropriate sign to keep values positive if sin(theta5) < 0: theta4 = atan2(-R3_6[2, 2], R3_6[0, 2]) theta6 = atan2(R3_6[1, 1], -R3_6[1, 0]) else: theta4 = atan2(R3_6[2, 2], -R3_6[0, 2]) theta6 = atan2(-R3_6[1, 1], R3_6[1, 0]) # Generate and print the error from the computed and expected end # effector # get the computed end effector using the computed thetas your_ee = T0_G.evalf( subs={ q1: theta1, q2: theta2, q3: theta3, q4: theta4, q5: theta5, q6: theta6 })[:3, 3] ee_x_e = abs(your_ee[0] - pwc_ee[0]) ee_y_e = abs(your_ee[1] - pwc_ee[1]) ee_z_e = abs(your_ee[2] - pwc_ee[2]) ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2) print("Computed EE: ") pprint(pwc_ee) print("Expected EE:") pprint(your_ee) print("End effector error for x position is: %04.8f" % ee_x_e) print("End effector error for y position is: %04.8f" % ee_y_e) print("End effector error for z position is: %04.8f" % ee_z_e) print("Overall end effector offset is: %04.8f units \n" % ee_offset) # Populate response for the IK request # In the next line replace theta1,theta2...,theta6 by your joint angle variables joint_trajectory_point.positions = [ theta1, theta2, theta3, theta4, theta5, theta6 ] joint_trajectory_list.append(joint_trajectory_point) rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list)) return CalculateIKResponse(joint_trajectory_list)
def interpolated_ik_motion_planner_callback(self, req): #names and angles for the joints in their desired order joint_names = req.motion_plan_request.start_state.joint_state.name start_angles = req.motion_plan_request.start_state.joint_state.position #sanity-checking: joint_names and start_angles should be the same length, if any start_angles are specified if start_angles and len(joint_names) != len(start_angles): rospy.logerr("start_state.joint_state.name needs to be the same length as start_state.joint_state.position! Quitting") return 0 #reorder the start angles to the order needed by IK reordered_start_angles = [] #get the current joint states for the robot #joint_states_msg = rospy.wait_for_message('joint_states', JointState, 10.0) #if not joint_states_msg: # rospy.logerr("unable to get joint_states message") # return 0 #get the desired start angles for each IK arm joint in turn from start_state.joint_state.position #(use the current angle if not specified) for joint_name in self.ik_utils.joint_names: #desired start angle specified if joint_name in joint_names and start_angles: index = joint_names.index(joint_name) reordered_start_angles.append(start_angles[index]) else: rospy.logerr("missing joint angle, can't deal") return 0 #desired start angle not specified, use the current angle #elif 0: #joint_name in joint_states_msg.name: # index = joint_states_msg.name.index(joint_name) # current_position = joint_states_msg.position[index] # reordered_start_angles.append(current_position) #malformed joint_states message? # else: # rospy.logerr("an expected arm joint,"+joint_name+"was not found!") # return 0 #get additional desired joint angles (such as for the gripper) to pass through to IK additional_joint_angles = [] additional_joint_names = [] for (ind, joint_name) in enumerate(joint_names): if joint_name not in self.ik_utils.joint_names: #rospy.loginfo("found %s"%joint_name) additional_joint_angles.append(start_angles[ind]) additional_joint_names.append(joint_name) IK_robot_state = None if additional_joint_angles: #rospy.loginfo("adding additional start angles for:"+str(additional_joint_names)) #rospy.loginfo("additional joint angles:"+str(additional_joint_angles)) IK_robot_state = RobotState() IK_robot_state.joint_state.name = additional_joint_names IK_robot_state.joint_state.position = additional_joint_angles #check that the desired link is in the list of possible IK links (only r/l_wrist_roll_link for now) link_name = req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids[0] if link_name != self.ik_utils.link_name: rospy.logerr("link_name not allowed: %s"%link_name) return 0 #the start pose for that link start_pose = req.motion_plan_request.start_state.multi_dof_joint_state.poses[0] #the frame that start pose is in frame_id = req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids[0] #turn it into a PoseStamped start_pose_stamped = self.add_header(PoseStamped(), frame_id) start_pose_stamped.pose = start_pose #the desired goal position goal_pos = req.motion_plan_request.goal_constraints.position_constraints[0].position #the frame that goal position is in goal_pos_frame = req.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id #convert the position to base_link frame goal_ps = self.add_header(PointStamped(), goal_pos_frame) goal_ps.point = goal_pos goal_pos_list = self.ik_utils.point_stamped_to_list(goal_ps, 'base_link') #the desired goal orientation goal_quat = req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation #the frame that goal orientation is in goal_quat_frame = req.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id #convert the quaternion to base_link frame goal_qs = self.add_header(QuaternionStamped(), goal_quat_frame) goal_qs.quaternion = goal_quat goal_quat_list = self.ik_utils.quaternion_stamped_to_list(goal_qs, 'base_link') #assemble the goal pose into a PoseStamped goal_pose_stamped = self.add_header(PoseStamped(), 'base_link') goal_pose_stamped.pose = Pose(Point(*goal_pos_list), Quaternion(*goal_quat_list)) #get the ordered collision operations, if there are any ordered_collision_operations = None #req.motion_plan_request.ordered_collision_operations #if ordered_collision_operations.collision_operations == []: # ordered_collision_operations = None #get the link paddings, if there are any link_padding = None #req.motion_plan_request.link_padding #if link_padding == []: # link_padding = None #RUN! Check the Cartesian path for consistent, non-colliding IK solutions (trajectory, error_codes) = self.ik_utils.check_cartesian_path(start_pose_stamped, \ goal_pose_stamped, reordered_start_angles, self.pos_spacing, self.rot_spacing, \ self.consistent_angle, self.collision_aware, self.collision_check_resolution, \ self.steps_before_abort, self.num_steps, ordered_collision_operations, \ self.start_from_end, IK_robot_state, link_padding) #find appropriate velocities and times for the valid part of the resulting joint path (invalid parts set to 0) #if we're searching from the end, keep the end; if we're searching from the start, keep the start start_ind = 0 stop_ind = len(error_codes) if self.start_from_end: for ind in range(len(error_codes)-1, 0, -1): if error_codes[ind]: start_ind = ind+1 break else: for ind in range(len(error_codes)): if error_codes[ind]: stop_ind = ind break (times, vels) = self.ik_utils.trajectory_times_and_vels(trajectory[start_ind:stop_ind], self.max_joint_vels, self.max_joint_accs) times = [0]*start_ind + times + [0]*(len(error_codes)-stop_ind) vels = [[0]*7]*start_ind + vels + [[0]*7]*(len(error_codes)-stop_ind) rospy.logdebug("trajectory:") for ind in range(len(trajectory)): rospy.logdebug("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) rospy.logdebug("") for ind in range(len(trajectory)): rospy.logdebug("time: " + "%5.3f "%times[ind] + "vels: " + self.pplist(vels[ind])) #the response res = GetMotionPlanResponse() #the arm joint names in the normal order, as spit out by IKQuery res.trajectory.joint_trajectory.joint_names = self.ik_utils.joint_names[:] #a list of 7-lists of joint angles, velocities, and times for a trajectory that gets you from start to goal #(all 0s if there was no IK solution for a point on the path) res.trajectory.joint_trajectory.points = [] for i in range(len(trajectory)): joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = trajectory[i] joint_trajectory_point.velocities = vels[i] joint_trajectory_point.time_from_start = rospy.Duration(times[i]) res.trajectory.joint_trajectory.points.append(joint_trajectory_point) #a list of ArmNavigationErrorCodes messages, one for each trajectory point, with values as follows: #ArmNavigationErrorCodes.SUCCESS (1): no problem #ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED (-23): no non-colliding IK solution (colliding solution provided) #ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED (-20): inconsistency in path between this point and the next point #ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED (-21): out of reach (no colliding solution) #ArmNavigationErrorCodes.PLANNING_FAILED (0): aborted before getting to this point error_code_dict = {0:ArmNavigationErrorCodes.SUCCESS, 1:ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED, \ 2:ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED, 3:ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED, \ 4:ArmNavigationErrorCodes.PLANNING_FAILED} trajectory_error_codes = [ArmNavigationErrorCodes(val=error_code_dict[error_code]) for error_code in error_codes] res.trajectory_error_codes = trajectory_error_codes res.error_code.val = ArmNavigationErrorCodes.SUCCESS # rospy.loginfo("trajectory:") # 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] + "vels: " + self.pplist(vels[ind])) return res
def handle_calculate_IK(req): rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses)) if len(req.poses) < 1: print "No valid poses received" return -1 else: ### Your FK code here # Create symbols # # # Create Modified DH parameters # # # Define Modified DH Transformation matrix # # # Create individual transformation matrices # # # Extract rotation matrices from the transformation matrices # # ### d6_val = 0.0 d1_val = 0.75 a1_val = 0.35 a2_val = 1.25 a3_val = -0.054 d4_val = 1.50 l = 0.303 q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols( 'alpha0:7') s = { alpha0: 0.0, a0: 0, d1: 0.75, q1: q1, alpha1: -pi / 2, a1: 0.35, d2: 0, q2: q2 - np.pi / 2, alpha2: 0.0, a2: 1.25, d3: 0, q3: q3, alpha3: -pi / 2, a3: -0.054, d4: 1.50, q4: q4, alpha4: pi / 2, a4: 0, d5: 0, q5: q5, alpha5: -pi / 2, a5: 0, d6: 0, q6: q6, alpha6: 0.0, a6: 0, d7: 0.303, q7: 0 } T0_1 = Matrix([[cos(q1), -sin(q1), 0, a0], [ sin(q1) * cos(alpha0), cos(q1) * cos(alpha0), -sin(alpha0), -sin(alpha0) * d1 ], [ sin(q1) * sin(alpha0), cos(q1) * sin(alpha0), cos(alpha0), cos(alpha0) * d1 ], [0, 0, 0, 1]]) T0_1 = T0_1.subs(s) T1_2 = Matrix([[cos(q2), -sin(q2), 0, a1], [ sin(q2) * cos(alpha1), cos(q2) * cos(alpha1), -sin(alpha1), -sin(alpha1) * d2 ], [ sin(q2) * sin(alpha1), cos(q2) * sin(alpha1), cos(alpha1), cos(alpha1) * d2 ], [0, 0, 0, 1]]) T1_2 = T1_2.subs(s) T2_3 = Matrix([[cos(q3), -sin(q3), 0, a2], [ sin(q3) * cos(alpha2), cos(q3) * cos(alpha2), -sin(alpha2), -sin(alpha2) * d3 ], [ sin(q3) * sin(alpha2), cos(q3) * sin(alpha2), cos(alpha2), cos(alpha2) * d3 ], [0, 0, 0, 1]]) T2_3 = T2_3.subs(s) """ T3_4 = Matrix( [[ cos(q4), -sin( q4), 0, a3], [ sin(q4)*cos(alpha3), cos(q4)*cos(alpha3), -sin(alpha3), - sin(alpha3)*d4], [ sin(q4)*sin(alpha3), cos(q4)*sin(alpha3), cos( alpha3), cos(alpha3)*d4], [ 0, 0, 0, 1]]) #T3_4 = T3_4. subs(s) T4_5 = Matrix( [[ cos(q5), -sin(q5), 0, a4] , [ sin(q5)*cos(alpha4), cos( q5 )*cos(alpha4), -sin(alpha4),-sin(alpha4)*d5], [ sin(q5)*sin( alpha4), cos(q5)*sin(alpha4), cos(alpha4), cos(alpha4)*d5], [ 0, 0, 0 , 1]]) #T4_5 = T4_5. subs(s) T5_6 =Matrix( [[ cos( q6), -sin(q6), 0, a5], [ sin(q6)* cos(alpha5) , cos(q6)*cos(alpha5), - sin(alpha5), -sin(alpha5)*d6], [ sin(q6)*sin(alpha5), cos(q6)*sin( alpha5), cos( alpha5), cos( alpha5)*d6], [ 0, 0, 0, 1]]) #T5_6 = T5_6.subs(s) T6_G = Matrix( [[ cos(q7), -sin(q7), 0, a6], [ sin(q7)*cos(alpha6), cos(q7)*cos(alpha6), -sin(alpha6), -sin(alpha6)*d7], [ sin(q7)*sin(alpha6), cos(q7)*sin(alpha6), cos(alpha6), cos(alpha6)*d7], [ 0, 0,0, 1]]) #T6_G = T6_G.subs(s) """ T0_2 = T0_1 * T1_2 T0_3 = T0_2 * T2_3 #T0_4 = simplify(T0_3 * T3_4) #T0_5 = simplify(T0_4 * T4_5) #T0_6 = simplify(T0_5 * T5_6) #T0_G = simplify(T0_6 * T6_G) R_z = Matrix([[cos(np.pi), -sin(np.pi), 0], [sin(np.pi), cos(np.pi), 0], [0, 0, 1]]) R_y = Matrix([[cos(-np.pi / 2), 0, sin(-np.pi / 2)], [0, 1, 0], [-sin(-np.pi / 2), 0, cos(-np.pi / 2)]]) R_corr = simplify(R_z * R_y) #T_total = simplify(T0_G * R_corr) # Initialize service response joint_trajectory_list = [] for x in xrange(0, len(req.poses)): # IK code starts here joint_trajectory_point = JointTrajectoryPoint() # Extract end-effector position and orientation from request # px,py,pz = end-effector position # roll, pitch, yaw = end-effector orientation px = req.poses[x].position.x py = req.poses[x].position.y pz = req.poses[x].position.z (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([ req.poses[x].orientation.x, req.poses[x].orientation.y, req.poses[x].orientation.z, req.poses[x].orientation.w ]) p1, p2, p3 = symbols('p1:4') Ry = rot_z(p1).subs({p1: yaw}) Rp = rot_y(p2).subs({p2: pitch}) Rr = rot_x(p3).subs({p3: roll}) Rrpy = simplify(Ry * Rp * Rr * R_corr) wx = px - (d6_val + l) * Rrpy[0, 2] wy = py - (d6_val + l) * Rrpy[1, 2] wz = pz - (d6_val + l) * Rrpy[2, 2] #print wx, wy, wz theta1 = atan2(wy, wx) #print theta1 A = sqrt(a3_val**2 + d4_val**2) B = sqrt((wz - d1_val)**2 + (sqrt(wy**2 + wx**2) - a1_val)**2) C = a2_val a = acos((-A**2 + B**2 + C**2) / (2 * C * B)) b = acos((A**2 - B**2 + C**2) / (2 * A * C)) c = acos((A**2 + B**2 - C**2) / (2 * A * B)) #print a, b, c theta2 = np.pi / 2 - a - atan2((wz - d1_val), (sqrt(wy**2 + wx**2) - a1_val)) theta3 = np.pi / 2 - (b - atan2(a3_val, d4_val)) #print theta1, theta2, theta3 R0_3 = T0_3[:3, :3] R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3}) R3_6 = R0_3.inv("LU") * Rrpy #R = (T3_4 * T4_5 * T5_6)[0:3, 0:3] #R = simplify(R) #print R """ for i in range(9): print R[i] print i if i+1 % 3 == 0: print "\n\n\n" """ theta4 = atan2(R3_6[2, 2], -R3_6[0, 2]) theta6 = atan2(-R3_6[1, 1], R3_6[1, 0]) fail = false theta5 = atan2(sqrt(R3_6[1, 0]**2 + R3_6[1, 1]**2), R3_6[1, 2]) theta5_old = 0 if np.abs(sin(theta5) * cos(theta6) - R3_6[1, 0]) > 1e-3 or np.abs( sin(theta5) * sin(theta6) + R3_6[1, 1]) > 1e-3: theta5_old = theta5 theta5 = atan2(-sqrt(R3_6[1, 0]**2 + R3_6[1, 1]**2), R3_6[1, 2]) if np.abs(sin(theta5) * cos(theta6) - R3_6[1, 0]) > 1e-3 or np.abs( sin(theta5) * sin(theta6) + R3_6[1, 1]) > 1e-3: rospy.logwarn("Solution not found for theta5") fail = true #print theta4, theta5, theta6 ### Your IK code here # Compensate for rotation discrepancy between DH parameters and Gazebo # # # Calculate joint angles using Geometric IK method # # ### # Populate response for the IK request # In the next line replace theta1,theta2...,theta6 by your joint angle variables if fail: ee_calculated1 = forward_kinematics( [theta1, theta2, theta3, theta4, theta5, theta6]) ee_calculated2 = forward_kinematics( [theta1, theta2, theta3, theta4, theta5_old, theta6]) # print ee_calculated error1 = [] error1.append(np.abs(ee_calculated1[0] - px)) error1.append(np.abs(ee_calculated1[1] - py)) error1.append(np.abs(ee_calculated1[2] - pz)) overall_err1 = sqrt(error1[0]**2 + error1[1]**2 + error1[2]**2) error2 = [] error2.append(np.abs(ee_calculated2[0] - px)) error2.append(np.abs(ee_calculated2[1] - py)) error2.append(np.abs(ee_calculated2[2] - pz)) overall_err2 = sqrt(error2[0]**2 + error2[1]**2 + error2[2]**2) if overall_err1 < overall_err2: joint_trajectory_point.positions = [ theta1, theta2, theta3, theta4, theta5, theta6 ] rospy.loginfo( str(sum(ee_calculated1)) + "\tError " + str(x) + "\t" + ": " + "\t" + str(error1) + "\tOverall error: " + str(overall_err1) + " other: " + str(overall_err2)) else: joint_trajectory_point.positions = [ theta1, theta2, theta3, theta4, theta5_old, theta6 ] rospy.loginfo( str(sum(ee_calculated2)) + "\tError " + str(x) + "\t" + ": " + "\t" + str(error2) + "\tOverall error: " + str(overall_err2) + " other: " + str(overall_err1)) else: joint_trajectory_point.positions = [ theta1, theta2, theta3, theta4, theta5, theta6 ] ee_calculated = forward_kinematics( joint_trajectory_point.positions) #print ee_calculated error = [] error.append(np.abs(ee_calculated[0] - px)) error.append(np.abs(ee_calculated[1] - py)) error.append(np.abs(ee_calculated[2] - pz)) overall_err = sqrt(error[0]**2 + error[1]**2 + error[2]**2) rospy.loginfo( str(sum(ee_calculated)) + "\tError " + str(x) + "\t" + ": " + "\t" + str(error) + "\tOverall error: " + str(overall_err)) joint_trajectory_list.append(joint_trajectory_point) rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list)) return CalculateIKResponse(joint_trajectory_list)
print 'moving the right arm out of the way' whicharm = 'r' arm_joint_trajectory_action_name = '' + whicharm + '_arm_controller/joint_trajectory_action' arm_joint_action_client = actionlib.SimpleActionClient( arm_joint_trajectory_action_name, JointTrajectoryAction) arm_joint_action_client.wait_for_server() arm_goal = JointTrajectoryGoal() arm_goal.trajectory.joint_names = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] arm_pt = JointTrajectoryPoint() arm_pt.positions = [-pi / 2, 0, 0, 0, 0, 0, 0] arm_pt.time_from_start = rospy.Duration(5.0) arm_goal.trajectory.points.append(arm_pt) arm_joint_action_client.send_goal_and_wait(arm_goal) print 'arm action state:', actionlib.get_name_of_constant( actionlib.GoalStatus, arm_joint_action_client.get_state()) print 'moving the left arm to problematic configuration' whicharm = 'l' arm_joint_trajectory_action_name = '' + whicharm + '_arm_controller/joint_trajectory_action' arm_joint_action_client = actionlib.SimpleActionClient( arm_joint_trajectory_action_name, JointTrajectoryAction) print 'waiting for', arm_joint_trajectory_action_name print 'have you set ROS_IP?'
def __init__(self): porcentaje = 0.7 #0.5 porcentaje1 = 0.7 #1 porcentaje16 = 0.8 # 1 des6 = 0.1 # 0.15 giro = 0 x = loadmat('estruc_piernas_fase3.mat') # print x piernas_der = x['estruc_piernaDer_fase'] art1_der = piernas_der[0, 0] art2_der = piernas_der[0, 1] art3_der = piernas_der[0, 2] art4_der = piernas_der[0, 3] art5_der = piernas_der[0, 4] art6_der = piernas_der[0, 5] piernas_izq = x['estruc_piernaIzq_fase'] art1_izq = piernas_izq[0, 0] art2_izq = piernas_izq[0, 1] art3_izq = piernas_izq[0, 2] art4_izq = piernas_izq[0, 3] art5_izq = piernas_izq[0, 4] art6_izq = piernas_izq[0, 5] 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.') arm_trajectory = JointTrajectory() arm_trajectory.joint_names = piernas_joints # 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) piernas_goalF01 = [ giro * porcentaje * art1_der['senialCompleta'][0, 50], 0.8 * porcentaje * art2_der['senialCompleta'][0, 50], porcentaje1 * art3_der['senialCompleta'][0, 50], porcentaje1 * art4_der['senialCompleta'][0, 50], 0.8 * porcentaje * art5_der['senialCompleta'][0, 50], porcentaje16 * art6_der['senialCompleta'][0, 50] + des6, giro * porcentaje * art1_izq['senialCompleta'][0, 50], 0.8 * porcentaje * art2_izq['senialCompleta'][0, 50], porcentaje1 * art3_izq['senialCompleta'][0, 50], porcentaje1 * art4_izq['senialCompleta'][0, 50], 0.8 * porcentaje * art5_izq['senialCompleta'][0, 50], porcentaje16 * art6_izq['senialCompleta'][0, 50] + des6 ] arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = piernas_goalF01 arm_trajectory.points[0].velocities = [0.0 for k in piernas_goalF01] arm_trajectory.points[0].accelerations = [0.0 for k in piernas_goalF01] arm_trajectory.points[0].time_from_start = rospy.Duration(2) ind = 1 salto = 4 for i in range(50, int(art1_der['senialCompleta'].shape[1] * 0.25), salto): # print ind # print int(art1_der['senialCompleta'].shape[1]*0.2) piernas_goalF = [ giro * porcentaje * art1_der['senialCompleta'][0, i], 0.8 * porcentaje * art2_der['senialCompleta'][0, i], porcentaje1 * art3_der['senialCompleta'][0, i], porcentaje1 * art4_der['senialCompleta'][0, i], 0.8 * porcentaje * art5_der['senialCompleta'][0, i], porcentaje16 * art6_der['senialCompleta'][0, i] + des6, giro * porcentaje * art1_izq['senialCompleta'][0, i], 0.8 * porcentaje * art2_izq['senialCompleta'][0, i], porcentaje1 * art3_izq['senialCompleta'][0, i], porcentaje1 * art4_izq['senialCompleta'][0, i], 0.8 * porcentaje * art5_izq['senialCompleta'][0, i], porcentaje16 * art6_izq['senialCompleta'][0, i] + des6 ] arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[ind].positions = piernas_goalF 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) arm_trajectory.points[ind].time_from_start = rospy.Duration( 2 + (porcentaje1 * art1_der['tiempo_completo'][0, i]) * 1.5) # print 1+art1_der['tiempo_completo'][0, i] ind += 1 # print art1_der['senialCompleta'][0, i] # 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.001) # 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')
# TODO: Add checking if action server is still alive # and manage reconnection (I think this is not automatic) if __name__ == '__main__': rospy.init_node('test_jtcm') # Launch PR2 simulation # roslaunch pr2_gazebo pr2_empty_world.launch jtcm = PR2JointTrajectoryControllerManager('/head_traj_controller') rospy.sleep(2) # Try to send a goal with all joints jt = JointTrajectory() jt.joint_names = ['head_pan_joint', 'head_tilt_joint'] p = JointTrajectoryPoint() for j in jt.joint_names: p.positions.append(0.3) p.velocities.append(0.1) p.time_from_start = rospy.Duration(1.0) jt.points.append(p) rospy.loginfo("Sending goal to both joints") jtcm.send_goal(jt) rospy.sleep(1.0) # Try to send a goal with extra joints that dont pertain from copy import deepcopy jt2 = deepcopy(jt) jt2.joint_names.append('non_existing_joint') jt2.points[0].positions[0] = -0.3
#!/usr/bin/env python import rospy, time from rospy.rostime import Duration from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint rospy.init_node('arm_traj_loop') left_arm_pub = rospy.Publisher('r2d2_left_arm_controller/command', JointTrajectory, queue_size=10) right_arm_pub = rospy.Publisher('r2d2_right_arm_controller/command', JointTrajectory, queue_size=10) waypoint1 = JointTrajectoryPoint() waypoint1.positions = [0.25, -1.5, 0.0] waypoint1.time_from_start = Duration(0.0) waypoint2 = JointTrajectoryPoint() waypoint2.positions = [0.75, -2.0, 0.75] waypoint2.time_from_start = Duration(1.0) waypoint3 = JointTrajectoryPoint() waypoint3.positions = [-0.15, -3.0, -1] waypoint3.time_from_start = Duration(3.0) waypoint4 = JointTrajectoryPoint() waypoint4.positions = [0, 0.5, 1] waypoint4.time_from_start = Duration(5.0) left_traj = JointTrajectory() left_traj.joint_names = ["left_shoulder", "left_elbow", "left_wrist"]
def handle_calculate_IK(req): """""" rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses)) if len(req.poses) < 1: print("No valid poses received") return -1 else: # Initialize service response joint_trajectory_list = [] for i in range(0, len(req.poses)): # IK code starts here joint_trajectory_point = JointTrajectoryPoint() # Extract end-effector position and orientation from request px = req.poses[i].position.x py = req.poses[i].position.y pz = req.poses[i].position.z (roll, pitch, yaw) = tf.transformations.euler_from_quaternion( [req.poses[i].orientation.x, req.poses[i].orientation.y, req.poses[i].orientation.z, req.poses[i].orientation.w]) # Rotation matrix from the base to the end-effector trans0_G = xyz_fixed_angle(roll, pitch, yaw)[:3, :3] # coordinates of the spherical wrist center dp = DG * np.matmul(trans0_G, np.array([1, 0, 0])) px_wc = px - dp[0, 0] py_wc = py - dp[0, 1] pz_wc = pz - dp[0, 2] # obtain thetas for the first three joints tht1, tht2, tht3 = solve_position_kinematics(px_wc, py_wc, pz_wc) trans0_3 = trans_base_link3(tht1, tht2, tht3)[:3, :3] # The inverse matrix of R0_3 is its transpose matrix trans3_G = np.matmul(trans0_3.T, trans0_G) # r11 = -sin(theta5)*cos(theta4) # r31 = sin(theta5)*sin(theta4) tht4 = np.arctan2(trans3_G[2, 0], -trans3_G[0, 0]) # r22 = sin(theta5)*sin(theta6) # r23 = sin(theta5)*cos(theta6) tht6 = np.arctan2(trans3_G[1, 1], trans3_G[1, 2]) # r12 = cos(theta5) # There are two possible solutions for theta5, but only one of them is correct tht5 = np.arctan2(np.sqrt(1 - trans3_G[1, 0] ** 2), trans3_G[1, 0]) if np.abs(np.sin(tht5) * np.sin(tht6) - trans3_G[1, 1]) < 1e-6 and \ np.abs(np.sin(tht5) * np.cos(tht6) - trans3_G[1, 2]) < 1e-6: pass else: tht5 = np.arctan2(-np.sqrt(1 - trans3_G[1, 0] ** 2), trans3_G[1, 0]) if np.abs(np.sin(tht5) * np.sin(tht6) - trans3_G[1, 1]) < 1e-6 and \ np.abs(np.sin(tht5) * np.cos(tht6) - trans3_G[1, 2]) < 1e-6: pass else: print("Warning: Not found! Solution for theta5!") # print("The solution is: \ntheta1 = {}, theta2 = {}, theta3 = {}\n" # "theta4 = {}, theta5 = {}, theta6 = {}". # format(tht1, tht2, tht3, tht4, tht5, tht6)) prediction = trans_base_linkg(tht1, tht2, tht3, tht4, tht5, tht6) print("px error: {:.4e}, py error {:.4e}, pz error {:.4e}". format(prediction[0, 3] - px, prediction[1, 3] - py, prediction[2, 3] - pz)) # Populate response for the IK request joint_trajectory_point.positions = [ tht1, tht2, tht3, tht4, tht5, tht6] joint_trajectory_list.append(joint_trajectory_point) rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list)) return CalculateIKResponse(joint_trajectory_list)
sub_joints = rospy.Subscriber('/arm_controller/state', JointTrajectoryControllerState, cb_arm_state) pub_control = rospy.Publisher("/arm_controller/command", JointTrajectory, queue_size=1) freq = 20 # Number of commands published per second joint_vel_max = 1.0 # Maximum velocity of a single joint - if this is exceeded all joint velocities are scaled down duration_mult = 2 # Velocities are set as points to reach in the future - this multiplies how far ahead to make that point. At 1 the goal is at (1/freq), causing a lot of jittery motion in the arm r = rospy.Rate(freq) msg_arm = JointTrajectory() msg_arm.joint_names = [ "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint", "arm_7_joint" ] msg_arm_point = JointTrajectoryPoint() msg_arm_point.time_from_start = Duration(nsecs=int(1e9 / freq * duration_mult)) joint_vel_command = np.ndarray( shape=(7, 1), dtype=float, buffer=np.array([1, 1, 0, 0, 0, 0, 0], dtype=float) * joint_vel_max) while not rospy.is_shutdown(): r.sleep() if (joint_angles is None): # Skip if not enough callbacks yet continue if np.all(np.equal(joint_vel_command, np.zeros(shape=(7, 1)))): # Skip if command is 0 continue
client_server = client.wait_for_server(rospy.Duration(10.0)) if not client_server: msg = ("Action server not available." " Verify action server availability.") rospy.logerr(msg) rospy.signal_shutdown(msg) sys.exit(1) # Creates a goal to send to the action server. goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = ['arm_elbow_joint',\ 'arm_shoulder_lift_joint', 'arm_shoulder_pan_joint',\ 'arm_wrist_1_joint', 'arm_wrist_2_joint', 'arm_wrist_3_joint'] goal.trajectory.points = [] point0 = JointTrajectoryPoint() point0.positions = [0.0,0.0,0.0,0.0,0.0,0.0] point0.velocities = [0.0,0.0,0.0,0.0,0.0,0.0] # To be reached 1 second after starting along the trajectory point0.time_from_start = rospy.Duration(1.0) goal.trajectory.points.append(point0) point1 = JointTrajectoryPoint() point1.positions = [0.05,0.05,0.05,0.05,0.05,0.03] point1.velocities = [0.0,0.0,0.0,0.0,0.0,0.0] point1.time_from_start = rospy.Duration(2.0) goal.trajectory.points.append(point1) # point2 = JointTrajectoryPoint() # point2.positions = [0.0,0.0,0.0,0.0,0.0,0.2] # point2.velocities = [0.0,0.0,0.0,0.0,0.0,0.0]
def handle_calculate_IK(req): rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses)) if len(req.poses) < 1: print("No valid poses received") return -1 else: start_time = time() ### Your FK code here # Create symbols # # α_{i-1} for Rx alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols( 'alpha0:7') # a_{i-1} for Dx a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') # θi for Rz, **Joint Angles** q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # di for Dz d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') # # Create Modified DH parameters # # Define dh parameters dh_params = { alpha0: 0, a0: 0, q1: q1, d1: 0.33 + 0.42, alpha1: -pi / 2, a1: 0.35, q2: q2 - pi / 2, d2: 0, alpha2: 0, a2: 1.25, q3: q3, d3: 0, alpha3: -pi / 2, a3: -0.054, q4: q4, d4: 0.96 + 0.54, alpha4: pi / 2, a4: 0, q5: q5, d5: 0, alpha5: -pi / 2, a5: 0, q6: q6, d6: 0, alpha6: 0, a6: 0, q7: 0, d7: 0.193 + 0.11, } # # Define Modified DH Transformation matrix # t1 = matrix_from(alpha0, a0, q1, d1).subs(dh_params) t2 = matrix_from(alpha1, a1, q2, d2).subs(dh_params) t3 = matrix_from(alpha2, a2, q3, d3).subs(dh_params) t4 = matrix_from(alpha3, a3, q4, d4).subs(dh_params) t5 = matrix_from(alpha4, a4, q5, d5).subs(dh_params) t6 = matrix_from(alpha5, a5, q6, d6).subs(dh_params) t7 = matrix_from(alpha6, a6, q7, d7).subs(dh_params) #t_ee = t1 * t2 * t3 * t4 * t5 * t6 * t7 # # Create individual transformation matrices # r, p, y = symbols('roll pitch yaw') # Generate rotation matrix rot_fix = rot_z(pi) * rot_y(-pi / 2) # Fix for URDF <-> DH-convertion _rot_ee = rot_z(y) * rot_y(p) * rot_x(r) * rot_fix ### # Initialize service response joint_trajectory_list = [] for x in xrange(0, len(req.poses)): start_each_time = time() # IK code starts here joint_trajectory_point = JointTrajectoryPoint() # Extract end-effector position and orientation from request # px,py,pz = end-effector position # roll, pitch, yaw = end-effector orientation px = req.poses[x].position.x py = req.poses[x].position.y pz = req.poses[x].position.z (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([ req.poses[x].orientation.x, req.poses[x].orientation.y, req.poses[x].orientation.z, req.poses[x].orientation.w ]) ### Your IK code here pos_ee = Matrix([px, py, pz]) # Compensate for rotation discrepancy between DH parameters and Gazebo rot_ee = _rot_ee.subs({r: roll, p: pitch, y: yaw}) pos_wc = pos_ee - dh_params[d7] * rot_ee[:, 2] # # Calculate joint angles using Geometric IK method # # Project the position to the plane, and thus obtain angle of (x, y) theta1 = atan2(pos_wc[1], pos_wc[0]).evalf() # Obtain side lengths of fig3 using pythagorean theorem fig3_side_a = pos_wc[0] # x fig3_side_b = pos_wc[1] # y fig3_side_c = sqrt(fig3_side_a**2 + fig3_side_b**2) # Obtain side lengths of fig2 using pythagorean theorem fig2_side_a = pos_wc[2] - dh_params[d1] # z - height of joint1 fig2_side_b = fig3_side_c - dh_params[a1] # side_b-offset fig2_side_c = sqrt(fig2_side_a**2 + fig2_side_b**2) # Obtain side lengths of fig1 using pythagorean theorem fig1_side_a = dh_params[d4] fig1_side_b = fig2_side_c fig1_side_c = dh_params[a2] ## Obtain angles of fig1 # # Get triangle angles using sides of triangle # # ref.low of cosines https://en.wikipedia.org/wiki/Law_of_cosines#Applications def angles_from_triangle_sides(a, b, c): """:a, b, c: side values of triangle""" gamma = acos((a**2 + b**2 - c**2) / (2 * a * b)) beta = acos((a**2 + c**2 - b**2) / (2 * a * c)) alpha = acos((b**2 + c**2 - a**2) / (2 * b * c)) return (alpha, beta, gamma) fig1_angle_a, fig1_angle_b, _ = angles_from_triangle_sides( fig1_side_a, fig1_side_b, fig1_side_c) theta2 = pi / 2 - fig1_angle_a - atan2(fig2_side_a, fig2_side_b).evalf() theta3 = pi / 2 - fig1_angle_b + atan2(dh_params[a3], dh_params[d4]).evalf() # # Extract rotation matrices from the transformation matrices # # Slice Rotation Matrix, i=1..3 rot0_3 = (t1 * t2 * t3)[0:3, 0:3].evalf(subs={ q1: theta1, q2: theta2, q3: theta3 }) # Obtain the Rotation Matrix, i=3..6 # Memo: # r: rotation_matrix # inv(r) == r^T rot3_6 = rot0_3.T * rot_ee #_rot3_6 = rot3_6 theta4, theta5, theta6 = euler_angles_from_rotation_matrix(rot3_6) # ### rospy.loginfo(" Each time[%d]: %04.4f seconds" % (x, time() - start_each_time)) # Populate response for the IK request # In the next line replace theta1,theta2...,theta6 by your joint angle variables joint_trajectory_point.positions = [ theta1, theta2, theta3, theta4, theta5, theta6 ] joint_trajectory_list.append(joint_trajectory_point) rospy.loginfo(" Total run time: %04.4f seconds" % (time() - start_time)) rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list)) return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req): rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses)) if len(req.poses) < 1: print "No valid poses received" return -1 else: ### Your FK code here T0_1,T1_2,T2_3,q1,q2,q3 = FK() # Initialize service response joint_trajectory_list = [] for x in xrange(0, len(req.poses)): # IK code starts here joint_trajectory_point = JointTrajectoryPoint() # Extract end-effector position and orientation from request # px,py,pz = end-effector position # roll, pitch, yaw = end-effector orientation px = req.poses[x].position.x py = req.poses[x].position.y pz = req.poses[x].position.z (roll, pitch, yaw) = tf.transformations.euler_from_quaternion( [req.poses[x].orientation.x, req.poses[x].orientation.y, req.poses[x].orientation.z, req.poses[x].orientation.w]) # Compensate for rotation discrepancy between DH parameters and Gazebo # Find EE rotation Matrix r,p,y = symbols('r p y') ROT_x = rot_x(r) # roll ROT_y = rot_y(p) # pitch ROT_z = rot_z(y) # yaw ROT_EE = ROT_z * ROT_y * ROT_x Rot_error = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90)) ROT_EE = ROT_EE * Rot_error #compenstate for error ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw}) EE = Matrix([[px], [py], [pz]]) WC = EE - (0.303) * ROT_EE[:,2] # wrist center # calculate joint angles using Geometric IK method theta1 = atan2(WC[1],WC[0]) # SSS triangle for theta2 and theta3 side_a = 1.501 side_b = sqrt(pow((sqrt(WC[0]*WC[0] + WC[1]*WC[1]) - 0.35), 2) + pow((WC[2] - 0.75), 2)) side_c = 1.25 angle_a = acos((side_b * side_b + side_c * side_c - side_a * side_a) / (2 * side_b * side_c)) angle_b = acos((side_a * side_a + side_c * side_c - side_b * side_b) / (2 * side_a * side_c)) angle_c = acos((side_a * side_a + side_b * side_b - side_c * side_c) / (2 * side_a * side_b)) theta2 = pi / 2 - angle_a - atan2(WC[2] - 0.75, sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35) theta3 = pi / 2 - (angle_b + 0.036) # sag in link 4 -0.054m # rotation matrices R0_3 = T0_1[0:3,0:3] * T1_2[0:3,0:3] * T2_3[0:3,0:3] R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3}) #R3_6 = R0_3.inv("LU") * ROT_EE R3_6 = R0_3.transpose() * ROT_EE #Eular angles from rotation matrix theta4 = atan2(R3_6[2,2], -R3_6[0,2]) theta5 = atan2(sqrt(R3_6[0,2] * R3_6[0,2] + R3_6[2,2] * R3_6[2,2]), R3_6[1,2]) theta6 = atan2(-R3_6[1,1], R3_6[1,0]) # Populate response for the IK request # In the next line replace theta1,theta2...,theta6 by your joint angle variables joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6] joint_trajectory_list.append(joint_trajectory_point) rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list)) return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req): rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses)) if len(req.poses) < 1: print "No valid poses received" return -1 else: ### Your FK code here # Create symbols q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols( 'alpha0:7') # Create Modified DH parameters s = { alpha0: 0, a0: 0, d1: 0.75, q1: q1, alpha1: -pi / 2., a1: 0.35, d2: 0, q2: q2 - pi / 2., alpha2: 0, a2: 1.25, d3: 0, q3: q3, alpha3: -pi / 2., a3: -0.054, d4: 1.50, q4: q4, alpha4: pi / 2., a4: 0, d5: 0, q5: q5, alpha5: -pi / 2., a5: 0, d6: 0, q6: q6, alpha6: 0, a6: 0, d7: 0.303, q7: 0 } # Define Modified DH Transformation matrix def TF_Matrix(alpha, a, d, q): TF = Matrix([[cos(q), -sin(q), 0, a], [ sin(q) * cos(alpha), cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d ], [ sin(q) * sin(alpha), cos(q) * sin(alpha), cos(alpha), cos(alpha) * d ], [0, 0, 0, 1]]) return TF # Create individual transformation matrices T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(s) T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(s) T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(s) T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(s) T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(s) T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(s) T6_EE = TF_Matrix(alpha6, a6, d7, q7).subs(s) # Extract rotation matrices from the transformation matrices T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE ### # Initialize service response joint_trajectory_list = [] for x in xrange(0, len(req.poses)): # IK code starts here joint_trajectory_point = JointTrajectoryPoint() # Extract end-effector position and orientation from request # px,py,pz = end-effector position # roll, pitch, yaw = end-effector orientation px = req.poses[x].position.x py = req.poses[x].position.y pz = req.poses[x].position.z (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([ req.poses[x].orientation.x, req.poses[x].orientation.y, req.poses[x].orientation.z, req.poses[x].orientation.w ]) ### Your IK code here # Define RPY rotation matrix r, p, y = symbols('r p y') ROT_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0, sin(r), cos(r)]]) ROT_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0.], [-sin(p), 0, cos(p)]]) ROT_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0, 1]]) # Compensate for rotation discrepancy between DH parameters and Gazebo ROT_EE = ROT_z * ROT_y * ROT_x Rot_Error = ROT_z.subs(y, radians(180)) * ROT_y.subs( p, radians(-90)) ROT_EE = ROT_EE * Rot_Error ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw}) EE = Matrix([[px], [py], [pz]]) WC = EE - (0.303) * ROT_EE[:, 2] # Calculate joint angles using Geometric IK method theta1 = atan2(WC[1], WC[0]) side_a = 1.501 side_b = sqrt( pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) + pow((WC[2] - 0.75), 2)) side_c = 1.25 angle_a = acos( (side_b * side_b + side_c * side_c - side_a * side_a) / (2 * side_b * side_c)) angle_b = acos( (side_a * side_a + side_c * side_c - side_b * side_b) / (2 * side_a * side_c)) angle_c = acos( (side_a * side_a + side_b * side_b - side_c * side_c) / (2 * side_a * side_b)) theta2 = pi / 2 - angle_a - atan2( WC[2] - 0.75, sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35) theta3 = pi / 2 - angle_b - 0.036 R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3] R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3}) R3_6 = R0_3.inv("LU") * ROT_EE # Euler angles from rotation matrix theta4 = atan2(R3_6[2, 2], -R3_6[0, 2]) theta5 = atan2( sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]), R3_6[1, 2]) theta6 = atan2(-R3_6[1, 1], R3_6[1, 0]) ### # Populate response for the IK request # In the next line replace theta1,theta2...,theta6 by your joint angle variables joint_trajectory_point.positions = [ theta1, theta2, theta3, theta4, theta5, theta6 ] joint_trajectory_list.append(joint_trajectory_point) rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list)) return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req): rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses)) if len(req.poses) < 1: print "No valid poses received" return -1 else: # Initialize service response joint_trajectory_list = [] for x in xrange(0, len(req.poses)): # IK code starts here joint_trajectory_point = JointTrajectoryPoint() # Extract end-effector position and orientation from request # px,py,pz = end-effector position # roll, pitch, yaw = end-effector orientation px = req.poses[x].position.x py = req.poses[x].position.y pz = req.poses[x].position.z (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([ req.poses[x].orientation.x, req.poses[x].orientation.y, req.poses[x].orientation.z, req.poses[x].orientation.w ]) # Calculate joint angles using Geometric IK method lx = cos(roll) * cos(pitch) ly = sin(roll) * cos(pitch) lz = -sin(pitch) gripper_length = 0.303 #d6 +d7 wx = px - (gripper_length) * lx wy = py - (gripper_length) * ly wz = pz - (gripper_length) * lz q1 = atan2(wy, wx) #q2 position x_2 = 0.35 * cos(q1) y_2 = 0.35 * sin(q1) z_2 = 0.75 a2 = 1.25 d3 = 1.5 #distance q2-q4 dist1 = sqrt((wx - x_2)**2.0 + (wy - y_2)**2.0 + (wz - z_2)**2.0) q3 = acos((a2**2.0 + d3**2.0 - dist1**2.0) / (2.0 * a2 * d3)) q3 = (q3 - pi / 2.0) * -1.0 #conversion to robot joint angle q2 = atan2(wz - z_2, sqrt((wx - x_2)**2 + (wy - y_2)**2)) + acos( (a2**2.0 - d3**2.0 + dist1**2.0) / (2.0 * a2 * dist1)) q2 = (q2 - pi / 2.0) * -1.0 #conversion to robot joint angle #Roll and pitch in wrist coordinates space roll2 = roll - q1 pitch2 = pitch - (q2 + q3) #End efector projection in axe dgry = 0.303 * cos(pitch2) * sin(roll2) dgrz = 0.303 * sin(pitch2) #wrist angles q4 = atan2(dgry, dgrz) q5 = atan2(sqrt(dgry**2 + dgrz**2), cos(pitch2) * cos(roll2)) #clip q5 to avoid forbbiden values if q5 > 2.18: q5 = 2.18 if q5 < -2.18: q5 = -2.18 if pitch > -pi / 2 and pitch < pi / 2: q6 = yaw - q4 else: q6 = pi - (yaw - q4) # Populate response for the IK request joint_trajectory_point.positions = [q1, q2, q3, q4, q5, q6] #rospy.loginfo("Angles: %s" % joint_trajectory_point.positions) joint_trajectory_list.append(joint_trajectory_point) rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list)) return CalculateIKResponse(joint_trajectory_list)
if lyap > 0.001: phy = 2 else: phy = 0 vel_commands = -phy*lyap_vctrl # test_vel = np.array([-0.2, 0, 0, 0, 0, 0]) # publish controller dt = time.time() - self.old_time self.old_time = time.time() target_pose = q + vel_commands*dt print ("Vel Controller:") print (vel_commands) print ('Joint_Config') print (q) self.joint_traj.points = [JointTrajectoryPoint(positions = q, velocities= -vel_commands, time_from_start=rospy.Duration(0.1))] self.sim_joint_command_pub.publish(self.joint_traj) def scale_translation(self, X): X[0:3, 3] = self.t_scale*X[0:3, 3] return X def get_best_target(self, X_H): X_T1 = self.get_target_pose('cube1_link', 'world') X_T2 = self.get_target_pose('cube2_link', 'world') X_T3 = self.get_target_pose('cube3_link', 'world') X_T_candidates = [X_T1, X_T2, X_T3] X_Ts = [] for X_T_temp in X_T_candidates:
def open_gripper(self): self.gripper_msg.points = [JointTrajectoryPoint(positions=[0.0], velocities=[0], time_from_start=rospy.Duration(1.0))] self.griper_pos.publish(self.gripper_msg)
def handle_calculate_IK(req): rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses)) if len(req.poses) < 1: print "No valid poses received" return -1 else: # Initialize service response joint_trajectory_list = [] # Define DH param symbols # link_offset_i: signed distance from x_(i-1) to x_i along z_i # link_length_(i-1): distance from z_(i-1) to z_i along x_i, where x_i is perpendicular to both z_(i-1) and z_i # twist_angle_(i-1): angle between z_(i-1) and z_i about x_(i-1) in the right hand sense d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') # link_offset_i a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') # link_length_(i-1) alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')# twist_angle_(i-1) # theta_i: joint angle between x_(i-1) and x_i about z_i in the right hand sense q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # theta_i # Modified DH params s = {alpha0: 0, a0: 0, d1: 0.75, alpha1: -pi/2, a1: 0.35, d2: 0, q2: q2-pi/2, alpha2: 0, a2: 1.25, d3: 0, alpha3: -pi/2, a3: -0.054, d4: 1.5, alpha4: pi/2, a4: 0, d5: 0, alpha5: -pi/2, a5: 0, d6: 0, alpha6: 0, a6: 0, d7: 0.303, q7: 0} # Create individual transformation matrices T0_1 = Transformation_Matrix(q1, alpha0, a0, d1) T0_1 = T0_1.subs(s) T1_2 = Transformation_Matrix(q2, alpha1, a1, d2) T1_2 = T1_2.subs(s) T2_3 = Transformation_Matrix(q3, alpha2, a2, d3) T2_3 = T2_3.subs(s) T0_3 = simplify(T0_1 * T1_2 * T2_3) # Intrinsic rotation correction for end-effector transformation matrix R_z_intrinsic = Matrix([[ cos(pi), -sin(pi), 0], [ sin(pi), cos(pi), 0], [ 0, 0, 1]]) R_y_intrinsic = Matrix([[ cos(-pi/2), 0, sin(-pi/2)], [ 0, 1, 0], [-sin(-pi/2), 0, cos(-pi/2)]]) ZY_intrinsic_rot = R_z_intrinsic * R_y_intrinsic for x in xrange(0, len(req.poses)): # IK code starts here joint_trajectory_point = JointTrajectoryPoint() # Extract end-effector position and orientation from request # px,py,pz = end-effector position # roll, pitch, yaw = end-effector orientation px = req.poses[x].position.x py = req.poses[x].position.y pz = req.poses[x].position.z (roll, pitch, yaw) = tf.transformations.euler_from_quaternion( [req.poses[x].orientation.x, req.poses[x].orientation.y, req.poses[x].orientation.z, req.poses[x].orientation.w]) # Calculate transformation matrix from base link to end-effector R_z_extrinsic = Matrix([[ cos(yaw), -sin(yaw), 0], [ sin(yaw), cos(yaw), 0], [ 0, 0, 1]]) R_y_extrinsic = Matrix([[ cos(pitch), 0, sin(pitch)], [ 0, 1, 0], [-sin(pitch), 0, cos(pitch)]]) R_x_extrinsic = Matrix([[ 1, 0, 0], [ 0, cos(roll), -sin(roll)], [ 0, sin(roll), cos(roll)]]) XYZ_extrinsic_rot = R_z_extrinsic * R_y_extrinsic * R_x_extrinsic R0_6 = XYZ_extrinsic_rot * ZY_intrinsic_rot # Define dictionary parameters for further use a_3 = s[a3] d_4 = s[d4] d_1 = s[d1] a_1 = s[a1] a_2 = s[a2] d_7 = s[d7] # Find Wrist Center Location wx = px - (d_7 * R0_6[0, 2]) wy = py - (d_7 * R0_6[1, 2]) wz = pz - (d_7 * R0_6[2, 2]) # Finding theta 1-3 theta1 = (atan2(wy, wx)).evalf() s1 = sqrt(wx**2 + wy**2) - a_1 s2 = wz - d_1 s3 = sqrt(s2**2 + s1**2) s4 = sqrt(a_3**2 + d_4**2) beta1 = atan2(s2, s1) D2 = (a_2**2 + s3**2 - s4**2) / (2 * a_2 * s3) beta2 = atan2(sqrt(1 - D2**2), D2) D3 = (a_2**2 + s4**2 - s3**2) / (2 * a_2 * s4) beta3 = atan2(sqrt(1 - D3**2), D3) beta4 = atan2(-a_3, d_4) theta2 = ((pi / 2) - beta2 - beta1).evalf() theta3 = ((pi / 2) - beta4 - beta3).evalf() # Finding theta 4-6 R0_3 = T0_3[0:3, 0:3] R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3}) R0_3_inv = R0_3.transpose() R3_6 = R0_3_inv * R0_6 r13 = R3_6[0, 2] r33 = R3_6[2, 2] r23 = R3_6[1, 2] r21 = R3_6[1, 0] r22 = R3_6[1, 1] r12 = R3_6[0, 1] r32 = R3_6[2, 1] theta5 = (atan2(sqrt(r13**2 + r33**2), r23)).evalf() theta4 = (atan2(r33, -r13)).evalf() theta6 = (atan2(-r22, r21)).evalf() print('wx: ' + str(wx)) print('wy: ' + str(wy)) print('wz: ' + str(wz)) print('theta1: ' + str(theta1)) print('theta2: ' + str(theta2)) print('theta3: ' + str(theta3)) print('theta4: ' + str(theta4)) print('theta5: ' + str(theta5)) print('theta6: ' + str(theta6)) # Populate response for the IK request # In the next line replace theta1,theta2...,theta6 by your joint angle variables joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6] joint_trajectory_list.append(joint_trajectory_point) rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list)) return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req): rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses)) if len(req.poses) < 1: print "No valid poses received" return -1 else: ### Your FK code here # Create symbols # link lengths a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') # link offsets d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') # Twist Angles(Alpha) alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7') rospy.loginfo("Created DH parameter symbols") # Joint angle symbols(Theta) q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') rospy.loginfo("Created Joint Angle symbols") # Create Modified DH parameters s = {alpha0: 0, a0: 0, d1: 0.75, alpha1: -pi / 2, a1: 0.35, d2: 0, q2: q2 - pi / 2, alpha2: 0, a2: 1.25, d3: 0, alpha3: -pi / 2, a3: -0.054, d4: 1.5, alpha4: pi / 2, a4: 0, d5: 0, alpha5: -pi / 2, a5: 0, d6: 0, alpha6: 0, a6: 0, d7: 0.303, q7: 0} rospy.loginfo("Created DH parameters") # Define Modified DH Transformation matrix # Create individual transformation matrices t0_1 = dh_transformation(alpha0, a0, q1, d1).subs(s) t1_2 = dh_transformation(alpha1, a1, q2, d2).subs(s) t2_3 = dh_transformation(alpha2, a2, q3, d3).subs(s) t3_4 = dh_transformation(alpha3, a3, q4, d4).subs(s) t4_5 = dh_transformation(alpha4, a4, q5, d5).subs(s) t5_6 = dh_transformation(alpha5, a5, q6, d6).subs(s) t6_7 = dh_transformation(alpha6, a6, q7, d7).subs(s) t0_7 = simplify(t0_1 * t1_2 * t2_3 * t3_4 * t4_5 * t5_6 * t6_7) rospy.loginfo("Created individuals transformation matrices") # Extract rotation matrices from the transformation matrices rot_corr = corr_matrix() calc_fwd_kin = simplify(t0_7 * rot_corr) t0_3 = simplify(t0_1 * t1_2 * t2_3) rot_0_3 = t0_3[0:3, 0:3] rospy.loginfo("Extracted rot matrices and calculated FK") # Initialize service response joint_trajectory_list = [] for x in xrange(0, len(req.poses)): # IK code starts here joint_trajectory_point = JointTrajectoryPoint() # Extract end-effector position and orientation from request # px,py,pz = end-effector position # roll, pitch, yaw = end-effector orientation px = req.poses[x].position.x py = req.poses[x].position.y pz = req.poses[x].position.z (roll, pitch, yaw) = tf.transformations.euler_from_quaternion( [req.poses[x].orientation.x, req.poses[x].orientation.y, req.poses[x].orientation.z, req.poses[x].orientation.w]) ### Your IK code here # Calculate joint angles using Geometric IK method roll, pitch, yaw = calculate_rot_rpw(roll, pitch, yaw) rot_0_6 = simplify(roll * pitch * yaw) p_end_effector = Matrix([[px], [py], [pz]]) joint5 = calc_wrist_center(p_end_effector, rot_0_6) theta1 = calc_theta1(joint5) theta3_intern, theta3 = calc_theta3(joint5, theta1) joint2 = get_joint2(theta1) theta2 = calc_theta2(joint2, joint5, theta3_intern) # Compensate for rotation discrepancy between DH parameters and Gazebo rot_0_3_matrix = rot_0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3}) rot_0_3_matrix_inverse = rot_0_3_matrix ** -1 rot_3_6 = rot_0_3_matrix_inverse * rot_0_6 theta6 = calc_theta6(rot_3_6) theta5 = calc_theta5(rot_3_6) theta4 = calc_theta4(rot_3_6) ### # Populate response for the IK request # In the next line replace theta1,theta2...,theta6 by your joint angle variables joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6] joint_trajectory_list.append(joint_trajectory_point) forward_kinematics = calc_fwd_kin.evalf( subs={q1: theta1, q2: theta2, q3: theta3, q4: theta4, q5: theta5, q6: theta6}) rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list)) return CalculateIKResponse(joint_trajectory_list)
def __init__(self): pi4 = 0.7853 filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/mi_robot_xacro4_cambio.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 posicionInicial_der_up_down[1] = 0.3 posicionInicial_der_up_down[2] = -0.3 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 posicionInicial_izq_up_down[1] = 0.3 posicionInicial_izq_up_down[2] = -0.3 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 posicionInicial_der_down_up[4] = 0.3 posicionInicial_der_down_up[3] = -0.3 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 posicionInicial_izq_down_up[4] = 0.3 posicionInicial_izq_down_up[3] = -0.3 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_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'] piernas_goal0 = [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 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_goal0[6] = q_out_izq_up_down[0] piernas_goal0[7] = q_out_izq_up_down[1] piernas_goal0[8] = q_out_izq_up_down[2] piernas_goal0[9] = q_out_izq_up_down[3] piernas_goal0[10] = q_out_izq_up_down[4] piernas_goal0[11] = q_out_izq_up_down[5] piernas_goal0[0] = q_out_der_up_down[0] piernas_goal0[1] = q_out_der_up_down[1] piernas_goal0[2] = q_out_der_up_down[2] piernas_goal0[3] = q_out_der_up_down[3] piernas_goal0[4] = q_out_der_up_down[4] piernas_goal0[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] #q_out_izq_up_down[0] -= pi4 piernas_goal12[6] = q_out_izq_up_down[0]-2*pi4 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] piernas_goal12[0] = 0 piernas_goal12[1] = 0 piernas_goal12[2] = -0.7 piernas_goal12[3] = 1.4 piernas_goal12[4] = 0 piernas_goal12[5] = -0.7 ######################################################### 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] piernas_goal2[6] = q_out_izq_up_down[0]-2*pi4 piernas_goal2[7] = q_out_izq_up_down[1]-0.3-0.3 piernas_goal2[8] = q_out_izq_up_down[2]-0.3 piernas_goal2[9] = q_out_izq_up_down[3]+0.6 piernas_goal2[10] = q_out_izq_up_down[4]+0.3+0.3 piernas_goal2[11] = q_out_izq_up_down[5] -0.3 piernas_goal2[0] = 0#-pi4 piernas_goal2[1] = -0.25 piernas_goal2[2] = -0.3 piernas_goal2[3] = 0.6 piernas_goal2[4] = 0.25 piernas_goal2[5] = -0.3 ################################################## piernas_goal3 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] piernas_goal3[6] = q_out_izq_up_down[0] +0.0 piernas_goal3[7] = q_out_izq_up_down[1] - 0.3 piernas_goal3[8] = q_out_izq_up_down[2] - 0.3 piernas_goal3[9] = q_out_izq_up_down[3] + 0.6 piernas_goal3[10] = q_out_izq_up_down[4] + 0.3 piernas_goal3[11] = q_out_izq_up_down[5] - 0.3 piernas_goal3[0] = -2*pi4 piernas_goal3[1] = -0.25 piernas_goal3[2] = -0.3 piernas_goal3[3] = 0.6 piernas_goal3[4] = 0.25 piernas_goal3[5] = -0.3 ################################################## piernas_goal4 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] piernas_goal4[6] = q_out_izq_up_down[0] +0.0 piernas_goal4[7] = q_out_izq_up_down[1] - 0.3 +0.3 piernas_goal4[8] = q_out_izq_up_down[2] - 0.3 +0.3 piernas_goal4[9] = q_out_izq_up_down[3] + 0.6 -0.6 piernas_goal4[10] = q_out_izq_up_down[4] + 0.3 -0.3 piernas_goal4[11] = q_out_izq_up_down[5] - 0.3 +0.3 piernas_goal4[0] = -2*pi4 piernas_goal4[1] = -0.25 piernas_goal4[2] = -0.7 piernas_goal4[3] = 1.4 piernas_goal4[4] = 0.25 piernas_goal4[5] = -0.7 # 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_goal0 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(3.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(6.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(9.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(12.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[4].positions = piernas_goal4 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(15.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[5].positions = piernas_goal12 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(18.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[6].positions = piernas_goal2 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(21.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[7].positions = piernas_goal3 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(24) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[8].positions = piernas_goal4 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(27) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[9].positions = piernas_goal12 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(30.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[10].positions = piernas_goal2 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(33.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[11].positions = piernas_goal3 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(36.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[12].positions = piernas_goal4 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(39.0) arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[13].positions = piernas_goal12 arm_trajectory.points[13].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[13].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[13].time_from_start = rospy.Duration(42.0) '''arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[14].positions = piernas_goal4 arm_trajectory.points[14].velocities = [0.0 for i in piernas_joints] arm_trajectory.points[14].accelerations = [0.0 for i in piernas_joints] arm_trajectory.points[14].time_from_start = rospy.Duration(32.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 traj_planner(self, cart_pos, grasp_step='move', way_points_number=10, movement='slow'): """Quintic Trajectory Planner Publish a trajectory to UR5 using quintic splines. Arguments: cart_pos {[float]} -- Grasp position [x, y, z] Keyword Arguments: grasp_step {str} -- Set UR5 movement type (default: {'move'}) way_points_number {number} -- Number of points considered in trajectory (default: {10}) movement {str} -- Movement speed (default: {'slow'}) """ offset_from_object = 0.05 if grasp_step == 'pregrasp': self.grasp_cartesian_pose = deepcopy(self.posCB) self.grasp_cartesian_pose[-1] += offset_from_object joint_pos = self.get_ik(self.grasp_cartesian_pose) if joint_pos is not None: joint_pos[-1] = self.ori self.final_orientation = deepcopy(self.ori) self.gripper_angle_grasp = deepcopy(self.d[-2]) elif grasp_step == 'grasp': self.grasp_cartesian_pose[-1] -= offset_from_object joint_pos = self.get_ik(self.grasp_cartesian_pose) if joint_pos is not None: joint_pos[-1] = self.final_orientation elif grasp_step == 'move': joint_pos = self.get_ik(cart_pos) if joint_pos is not None: joint_pos[-1] = 0.0 if joint_pos is not None: if movement == 'slow': final_traj_duration = 500.0 # total iteractions elif movement == 'fast': final_traj_duration = 350.0 v0 = a0 = vf = af = 0 t0 = 5.0 tf = (t0 + final_traj_duration) / way_points_number # tf by way point t = tf / 10 # for each movement ta = tf / 10 # to complete each movement a = [0.0] * 6 pos_points, vel_points, acc_points = [0.0] * 6, [0.0] * 6, [0.0 ] * 6 goal = self.__build_goal_message_ur5() for i in range(6): q0 = self.actual_position[i] qf = joint_pos[i] b = np.array([q0, v0, a0, qf, vf, af]).transpose() m = np.array([[1, t0, t0**2, t0**3, t0**4, t0**5], [0, 1, 2 * t0, 3 * t0**2, 4 * t0**3, 5 * t0**4], [0, 0, 2, 6 * t0, 12 * t0**2, 20 * t0**3], [1, tf, tf**2, tf**3, tf**4, tf**5], [0, 1, 2 * tf, 3 * tf**2, 4 * tf**3, 5 * tf**4], [0, 0, 2, 6 * tf, 12 * tf**2, 20 * tf**3]]) a[i] = np.linalg.inv(m).dot(b) for i in range(way_points_number): for j in range(6): pos_points[j] = a[j][0] + a[j][1] * t + a[j][2] * t**2 + a[ j][3] * t**3 + a[j][4] * t**4 + a[j][5] * t**5 vel_points[j] = a[j][1] + 2 * a[j][2] * t + 3 * a[j][ 3] * t**2 + 4 * a[j][4] * t**3 + 5 * a[j][5] * t**4 acc_points[j] = 2 * a[j][2] + 6 * a[j][3] * t + 12 * a[j][ 4] * t**2 + 20 * a[j][5] * t**3 goal.trajectory.points.append( JointTrajectoryPoint( positions=pos_points, velocities=vel_points, accelerations=acc_points, time_from_start=rospy.Duration(t))) #default 0.1*i + 5 t += ta self.client.send_goal(goal) self.all_close(joint_pos) else: print('Could not find a IK solution')
def move_arm( positions, time_from_start = 3.0, arm = 'r', client = None ): if (client == None): client = init_jt_client(arm) else: arm = client.action_client.ns[0:1]; # ignore arm argument rospy.loginfo( 'move arm \'%s\' to position %s'%( arm, positions ) ) joint_names = get_joint_names( ''.join( ( arm, '_arm_controller' ) ) ) jt = JointTrajectory() jt.joint_names = joint_names jt.points = [] jp = JointTrajectoryPoint() jp.time_from_start = rospy.Time.from_seconds( time_from_start ) jp.positions = positions jt.points.append( jp ) # push trajectory goal to ActionServer jt_goal = JointTrajectoryGoal() jt_goal.trajectory = jt jt_goal.trajectory.header.stamp = rospy.Time.now() # + rospy.Duration.from_sec( time_from_start ) client.send_goal( jt_goal ) client.wait_for_result() return client.get_state()