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 scale_trajectory_speed(traj, scale): # Create a new trajectory object new_traj = RobotTrajectory() # Initialize the new trajectory to be the same as the planned trajectory new_traj.joint_trajectory = traj.joint_trajectory # Get the number of joints involved n_joints = len(traj.joint_trajectory.joint_names) # Get the number of points on the trajectory n_points = len(traj.joint_trajectory.points) # Store the trajectory points points = list(traj.joint_trajectory.points) # Cycle through all points and scale the time from start, speed and acceleration for i in range(n_points): point = JointTrajectoryPoint() point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale point.velocities = list(traj.joint_trajectory.points[i].velocities) point.accelerations = list(traj.joint_trajectory.points[i].accelerations) point.positions = traj.joint_trajectory.points[i].positions for j in range(n_joints): point.velocities[j] = point.velocities[j] * scale point.accelerations[j] = point.accelerations[j] * scale * scale points[i] = point # Assign the modified points to the new trajectory new_traj.joint_trajectory.points = points # Return the new trajectory return new_traj
def build_traj(start, end, duration): if sorted(start.name) <> sorted(end.name): rospy.logerr('Start and End position joint_names mismatch') raise # assume the start-position joint-ordering joint_names = start.name # 始点定義 start_pt = JointTrajectoryPoint() start_pt.positions = start.position start_pt.velocities = [0]*len(start.position) start_pt.time_from_start = rospy.Duration(0.0) # 始点なので、待ち時間 = 0 [sec] # 中間地点定義 middle_pt = JointTrajectoryPoint() for j in joint_names: idx = end.name.index(j) middle_pt.positions.append((start.position[idx] + end.position[idx])/2.0) middle_pt.velocities.append(0) middle_pt.time_from_start = rospy.Duration(duration) # 中間地点までの到達時間 = duration [sec] # 執着地点 end_pt = JointTrajectoryPoint() for j in joint_names: idx = end.name.index(j) end_pt.positions.append(end.position[idx]) end_pt.velocities.append(0.0) end_pt.time_from_start = rospy.Duration(duration*2) # 終点までの到達時間 = duration*2 [sec] return JointTrajectory(joint_names=joint_names, points=[start_pt, middle_pt, end_pt])
def main(): pub = rospy.Publisher('sevenbot/joint_traj_controller/trajectory_command',JointTrajectory) rospy.init_node('traj_test') p1 = JointTrajectoryPoint() p1.positions = [0,0,0,0,0,0,0] p1.velocities = [0,0,0,0,0,0,0] p1.accelerations = [0,0,0,0,0,0,0] p2 = JointTrajectoryPoint() p2.positions = [0,0,1.0,0,-1.0,0,0] p2.velocities = [0,0,0,0,0,0,0] p2.accelerations = [0,0,0,0,0,0,0] p2.time_from_start = rospy.Time(4.0) p3 = JointTrajectoryPoint() p3.positions = [0,0,-1.0,0,1.0,0,0] p3.velocities = [0,0,0,0,0,0,0] p3.accelerations = [0,0,0,0,0,0,0] p3.time_from_start = rospy.Time(20.0) traj = JointTrajectory() traj.joint_names = ['j1','j2','j3','j4','j5','j6','j7'] traj.points = [p1,p2,p3] rospy.loginfo(traj) r = rospy.Rate(1) # 10hz pub.publish(traj) r.sleep() pub.publish(traj)
def build_traj(start, end, duration): # print start.name # print end.name if sorted(start.name) <> sorted(end.name): rospy.logerr('Start and End position joint_names mismatch') raise # assume the start-position joint-ordering joint_names = start.name start_pt = JointTrajectoryPoint() start_pt.positions = start.position start_pt.velocities = [0.05]*len(start.position) start_pt.time_from_start = rospy.Duration(0.0) end_pt = JointTrajectoryPoint() # end_pt = start_pt; # end_pt.positions[3] = end_pt.positions[3]+.1 for j in joint_names: idx = end.name.index(j) end_pt.positions.append(end.position[idx]) # reorder to match start-pos joint ordering end_pt.velocities.append(0) end_pt.time_from_start = rospy.Duration(duration) return JointTrajectory(joint_names=joint_names, points=[start_pt, end_pt])
def dual_arm_test(): pub = rospy.Publisher('/lwr/lbr_controller/command', JointTrajectory) cob_pub = rospy.Publisher('/cob3_1/lbr_controller/command', JointTrajectory) rospy.init_node('dual_arm_test') rospy.sleep(1.0) while not rospy.is_shutdown(): traj = JointTrajectory() traj.joint_names = ['lbr_1_joint', 'lbr_2_joint', 'lbr_3_joint', 'lbr_4_joint', 'lbr_5_joint', 'lbr_6_joint', 'lbr_7_joint'] ptn = JointTrajectoryPoint() ptn.positions = [0.5, 0.8, 0.8, -0.8, 0.8, 0.8, -0.3] ptn.velocities = [0.4, 0.4, 0.8, 0.1, 0.8, 0.8, 0.1] ptn.time_from_start = rospy.Duration(2.0) traj.header.stamp = rospy.Time.now() traj.points.append(ptn) pub.publish(traj) cob_pub.publish(traj) rospy.sleep(3.0) ptn.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] ptn.velocities = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] ptn.time_from_start = rospy.Duration(2.0) traj.points = [] traj.header.stamp = rospy.Time.now() traj.points.append(ptn) pub.publish(traj) cob_pub.publish(traj) rospy.sleep(3.0)
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 readInPoses(self): poses = rospy.get_param('~poses') rospy.loginfo("Found %d poses on the param server", len(poses)) for key,value in poses.iteritems(): try: # TODO: handle multiple points in trajectory trajectory = JointTrajectory() trajectory.joint_names = value["joint_names"] point = JointTrajectoryPoint() point.time_from_start = Duration(value["time_from_start"]) point.positions = value["positions"] trajectory.points = [point] self.poseLibrary[key] = trajectory except: rospy.logwarn("Could not parse pose \"%s\" from the param server:", key); rospy.logwarn(sys.exc_info()) # add a default crouching pose: if not "crouch" in self.poseLibrary: trajectory = JointTrajectory() trajectory.joint_names = ["Body"] point = JointTrajectoryPoint() point.time_from_start = Duration(1.5) point.positions = [0.0,0.0, # head 1.545, 0.33, -1.57, -0.486, 0.0, 0.0, # left arm -0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # left leg -0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # right leg 1.545, -0.33, 1.57, 0.486, 0.0, 0.0] # right arm trajectory.points = [point] self.poseLibrary["crouch"] = trajectory;
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 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 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 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 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 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 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 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 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 execute(self, userdata): rospy.loginfo('In create_move_group_joints_goal') _move_joint_goal = FollowJointTrajectoryGoal() _move_joint_goal.trajectory.joint_names = userdata.move_joint_list jointTrajectoryPoint = JointTrajectoryPoint() jointTrajectoryPoint.positions = userdata.move_joint_poses for x in range(0, len(userdata.move_joint_poses)): jointTrajectoryPoint.velocities.append(0.0) jointTrajectoryPoint.time_from_start = rospy.Duration(2.0) _move_joint_goal.trajectory.points.append(jointTrajectoryPoint) for joint in _move_joint_goal.trajectory.joint_names: goal_tol = JointTolerance() goal_tol.name = joint goal_tol.position = 5.0 goal_tol.velocity = 5.0 goal_tol.acceleration = 5.0 _move_joint_goal.goal_tolerance.append(goal_tol) _move_joint_goal.goal_time_tolerance = rospy.Duration(2.0) _move_joint_goal.trajectory.header.stamp = rospy.Time.now() rospy.loginfo('Move_Joint_GOAL: ' + str(_move_joint_goal)) userdata.move_joint_goal = _move_joint_goal return 'succeeded'
def create_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 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 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 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 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_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 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 traj_speed_up(traj, spd=3.0): new_traj = RobotTrajectory() new_traj = traj n_joints = len(traj.joint_trajectory.joint_names) n_points = len(traj.joint_trajectory.points) points = list(traj.joint_trajectory.points) for i in range(n_points): point = JointTrajectoryPoint() point.time_from_start = traj.joint_trajectory.points[i].time_from_start / spd point.velocities = list(traj.joint_trajectory.points[i].velocities) point.accelerations = list(traj.joint_trajectory.points[i].accelerations) point.positions = traj.joint_trajectory.points[i].positions for j in range(n_joints): point.velocities[j] = point.velocities[j] * spd point.accelerations[j] = point.accelerations[j] * spd * spd points[i] = point new_traj.joint_trajectory.points = points return new_traj
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 _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 point(): global param_change_flag global transition global ang1v, ang2v, vel_1v, vel_2v rospy.init_node('point_control') pub = rospy.Publisher('/arm_twist', JointTrajectoryPoint, queue_size=10) pub_fp = rospy.Publisher('/arm_fp', Float64, queue_size=10) pub_pv1 = rospy.Publisher('/pv1', Float64, queue_size=10) pub_pn1 = rospy.Publisher('/pn1', Float64, queue_size=10) pub_mix1 = rospy.Publisher('/mix1', Float64, queue_size=10) rospy.Subscriber('/arm_start_stop', Bool, run_cb) rospy.Subscriber("/offset_joint_azim", UInt16, offset_joint_azim_cb) rospy.Subscriber("/offset_joint_elev", UInt16, offset_joint_elev_cb) rospy.Subscriber("/vel_1", Float64, vel_1_cb) rospy.Subscriber("/vel_2", Float64, vel_2_cb) grados_joint_azim = rospy.get_param("/grados_joint_azim", 360) rospy.loginfo("%s is %s", rospy.resolve_name('grados_joint_azim'), grados_joint_azim) grados_joint_elev = rospy.get_param("/grados_joint_elev", 45) rospy.loginfo("%s is %s", rospy.resolve_name("/grados_joint_elev"),grados_joint_elev) ang1 = grados_joint_azim*np.pi/180 #np.pi*2 ang2 = grados_joint_elev*np.pi/180 #np.pi*3/4 offset_joint_azim = rospy.get_param("/offset_joint_azim", 0) rospy.loginfo("%s is %s", rospy.resolve_name("/offset_joint_azim"), offset_joint_azim) offset_joint_elev = rospy.get_param("/offset_joint_elev", 0) rospy.loginfo("%s is %s", rospy.resolve_name("/offset_joint_elev"), offset_joint_elev) ang1_offset = offset_joint_azim*np.pi/180 #np.pi*2 ang2_offset = offset_joint_elev*np.pi/180 #np.pi*3/4 pub_period = 0.05 #vel_1 = rospy.get_param("/vel_1") #rospy.loginfo("%s is %s", rospy.resolve_name('/vel_1'),vel_1) #vel_2 = rospy.get_param("/vel_2") #rospy.loginfo("%s is %s", rospy.resolve_name('/vel_2'),vel_2) start_time = time.time() time.sleep(0.01) next_point_time = 0 dt = 0 tau = 1.0 while not rospy.is_shutdown(): if(time.time() > next_point_time) and run_var==True: dt = time.time() - start_time if param_change_flag == True: param_change_flag = False transition = True dtv = dt if transition == True: fp = ((6/tau**5)*((dt-dtv)**5)) -((15/tau**4)*(dt-dtv)**4)+ ((10/tau**3)*(dt-dtv)**3) if fp < 1: #Transcion p = JointTrajectoryPoint() # pv1 = ang1v*np.sin((vel_1v*dt)*2*np.pi) + ang1_offset pn1 = ang1*np.sin((vel_1*dt)*2*np.pi) + ang1_offset pvv1 = vel_1v*2*np.pi*ang1v*np.cos((vel_1v*(dt-pub_period))*2*np.pi) pnv1 = vel_1*2*np.pi*ang1*np.cos((vel_1*(dt-pub_period))*2*np.pi) pv2 = ang2v*np.sin((vel_2v*dt)*2*np.pi) + ang2_offset pn2 = ang2*np.sin((vel_2*dt)*2*np.pi) + ang2_offset pvv2 = vel_2v*2*np.pi*ang2v*np.cos((vel_2v*dt)*2*np.pi) pnv2 = vel_2*2*np.pi*ang2*np.cos((vel_2*dt)*2*np.pi) ##Position# mix1 = fp*pn1 + (1-fp)*pv1 mix2 = fp*pn2 + (1-fp)*pv2 ##Vel## mixv1 = fp*pnv1 + (1-fp)*pvv1 mixv2 = fp*pnv2 + (1-fp)*pvv2 i = mix1 j = mix2 k = mixv1 l = mixv2 p.positions = [i,j] p.velocities = [k,l] p.accelerations = [0,0] p.time_from_start = rospy.Time.now() rospy.loginfo("point trans \n %s ", p) rospy.loginfo(fp) pub.publish(p) ### graficar ### pub_pv1.publish(pv1) pub_pn1.publish(pn1) pub_mix1.publish(mix1) pub_fp.publish(fp) ### else: transition = False else: #Estado Estable #Dentro de transicion OJO no sabemso si la funcion es monotonica p = JointTrajectoryPoint() i = ang1*np.sin((vel_1*dt)*2*np.pi) + ang1_offset j = ang2*np.sin((vel_2*dt)*2*np.pi) + ang2_offset k = vel_1*2*np.pi*ang1*np.cos((vel_1*(dt-pub_period))*2*np.pi) l = vel_2*2*np.pi*ang2*np.cos((vel_2*dt)*2*np.pi) vel_1v = vel_1 vel_2v = vel_2 dtv = dt ang1v = ang1 ang2v = ang2 p.positions = [i,j] p.velocities = [k,l] p.accelerations = [0,0] p.time_from_start = rospy.Time.now() rospy.loginfo("point \n %s ", p) pub.publish(p) next_point_time = time.time() + pub_period elif run_var == False: start_time = time.time() + dt
def stage_6(self): # Reverse turtlebot a specfic distance and lower arm at a specific velocity to target position rospy.loginfo("Begin stage 6") # Publisher to control the robot's speed self.cmd_vel_1 = rospy.Publisher('turtlebot_1/cmd_vel_mux/input/navi', Twist) self.cmd_vel_2 = rospy.Publisher('turtlebot_2/cmd_vel_mux/input/navi', Twist) # Create a joint state publisher self.arm_pub_1 = rospy.Publisher('turtlebot_1/arm_controller/command', JointTrajectory) self.arm_pub_2 = rospy.Publisher('turtlebot_2/arm_controller/command', JointTrajectory) # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer 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 support position rospy.loginfo("Position arm in support 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.0, -1.0, 1.0, -1.8] point.velocities = [2.0, 2.0, 20.0, 10.0, 1.0] point.time_from_start = rospy.Duration(4.0) msg.points.append(point) # Define joint trajectory points point = JointTrajectoryPoint() point.positions = [0.0, 1.5, 0.0, 0.0, -1.8] point.velocities = [10.0, 10.0, 10.0, 10.0, 1.0] point.time_from_start = rospy.Duration(8.0) msg.points.append(point) # publish joint stage message self.arm_pub_1.publish(msg) self.arm_pub_2.publish(msg) #rospy.sleep(3) # Reverse turtlebot and open skirt door # How fast will we update the robot's movement? in Hz rate = 20 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Set the forward linear speed to 0.1 meters per second linear_speed = 0.1 # Set the travel distance in meters goal_distance = 0.49 # Set the odom frame self.odom_frame = 'turtlebot_1/odom' self.base_frame = 'turtlebot_1/base_footprint' # Initialize the position variable as a Point type position_1 = Point() # Initialize the movement command move_cmd_1 = Twist() # Set the movement command to reverse motion move_cmd_1.linear.x = -linear_speed # Get the starting position values (position_1, rotation) = get_odom(self) x_start_1 = position_1.x y_start_1 = position_1.y # Keep track of the distance traveled distance_1 = 0 #################################################################################################### # Set the odom frame self.odom_frame = 'turtlebot_2/odom' self.base_frame = 'turtlebot_2/base_footprint' # Initialize the position variable as a Point type position_2 = Point() # Initialize the movement command move_cmd_2 = Twist() # Set the movement command to reverse motion move_cmd_2.linear.x = -linear_speed # Get the starting position values (position_2, rotation) = get_odom(self) x_start_2 = position_2.x y_start_2 = position_2.y # Keep track of the distance traveled distance_2 = 0 rospy.loginfo("Begin moving turtlebot backwards") # Enter the loop to move turtlebot while distance_1 < goal_distance or distance_2 < goal_distance and not rospy.is_shutdown( ): # Publish the Twist message and sleep 1 cycle self.cmd_vel_1.publish(move_cmd_1) self.cmd_vel_2.publish(move_cmd_2) # sleep r.sleep() # Get the current position # Set the odom frame self.odom_frame = 'turtlebot_1/odom' self.base_frame = 'turtlebot_1/base_footprint' (position_1, rotation_1) = get_odom(self) # Set the odom frame self.odom_frame = 'turtlebot_2/odom' self.base_frame = 'turtlebot_2/base_footprint' (position_2, rotation_2) = get_odom(self) # Compute the Euclidean distance from the start distance_1 = sqrt( pow((position_1.x - x_start_1), 2) + pow((position_1.y - y_start_1), 2)) distance_2 = sqrt( pow((position_2.x - x_start_2), 2) + pow((position_2.y - y_start_2), 2)) # Check whether we need to stop one of the turtlebots early if distance_1 >= goal_distance: move_cmd_1 = Twist() self.cmd_vel_1.publish(move_cmd_1) if distance_2 >= goal_distance: move_cmd_2 = Twist() self.cmd_vel_2.publish(move_cmd_2) # Stop the robot for good rospy.loginfo("Stop turtlebots") move_cmd_1 = Twist() self.cmd_vel_1.publish(move_cmd_1) move_cmd_2 = Twist() self.cmd_vel_2.publish(move_cmd_2) rospy.sleep(1)
def trajectory_test(self): self.scene.remove_attached_object(self.eef_link, name="object") self.scene.remove_world_object("object") #Define how to perform trajectory planning for robot-to-human grasping. Cartesian = True indicates cartesian path planning, #smoothed = True includes post-processing to apply time parameterization with external node. cartesian = True smoothed = True while (not self.environment_set): #Prepare the collision environment with respect to the marker self.scene.remove_world_object("table") self.scene.remove_world_object("right_restrict") self.scene.remove_world_object("left_restrict") self.scene.remove_world_object("top_restrict") try: jt = JointTrajectory() jt.joint_names = ['head_1_joint', 'head_2_joint'] jtp = JointTrajectoryPoint() jtp.positions = [0, -0.26] looks before jtp.time_from_start = rospy.Duration(1.0) head jt.points.append(jtp) self.head_cmd.publish(jt) #Look up to see the marker rospy.sleep(1) (trans,rot) = self.listener.lookupTransform('/base_footprint', '/arm_goal', rospy.Time(0)) #Collision object: table object_pose = geometry_msgs.msg.PoseStamped() object_pose.header.frame_id = "aruco_marker_frame" object_pose.pose.position.x=-self.object[0] - 0.05 object_pose.pose.position.y=-0.7 object_pose.pose.orientation.w=1.0 object_name = "table" self.scene.add_box(object_name, object_pose, size=(0.1,2,2)) #Collision object: left block obstacle_pose = geometry_msgs.msg.PoseStamped() obstacle_pose.header.frame_id = "aruco_marker_frame" obstacle_pose.pose.position.y=-0.5 obstacle_pose.pose.position.z = 1.7 obstacle_pose.pose.orientation.w=1.0 object_name = "left_restrict" self.scene.add_box(object_name, obstacle_pose, size=(2,2,2)) #Collision object: top block obstacle_pose = geometry_msgs.msg.PoseStamped() obstacle_pose.header.frame_id = "aruco_marker_frame" obstacle_pose.pose.position.x=1.5 obstacle_pose.pose.orientation.w=1.0 object_name = "top_restrict" self.scene.add_box(object_name, obstacle_pose, size=(2,2,2)) #Collision object: right block obstacle_pose = geometry_msgs.msg.PoseStamped() obstacle_pose.header.frame_id = "aruco_marker_frame" obstacle_pose.pose.position.y=-0.5 obstacle_pose.pose.position.z = -1.2 obstacle_pose.pose.orientation.w=1.0 object_name = "right_restrict" self.scene.add_box(object_name, obstacle_pose, size=(2,2,2)) #Collision object: object object_pose = geometry_msgs.msg.PoseStamped() object_pose.header.frame_id = "aruco_marker_frame" object_pose.pose.position.x= -0.1 object_pose.pose.orientation.w=1.0 object_name = "object" self.scene.remove_attached_object(self.eef_link, name="object") self.scene.remove_world_object("object") #Set initial object position (trans2,rot2) = self.listener.lookupTransform('/base_footprint', '/place_goal', rospy.Time(0)) self.initial_pose = [trans2[0], trans2[1], trans2[2]] self.initial_or = [rot2[0], rot2[1], rot2[2], rot2[3]] print ("initial pose of object",trans2, rot2) goal = PlayMotionGoal() goal.motion_name = 'open_gripper' goal.skip_planning = False self.play_m_as.send_goal_and_wait(goal) self.environment_set = True print("ENVIRONMENT READY") except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue jt = JointTrajectory() jt.joint_names = ['head_1_joint', 'head_2_joint'] jtp = JointTrajectoryPoint() jtp.positions = [0, -0.75] jtp.time_from_start = rospy.Duration(1.0) jt.points.append(jtp) self.head_cmd.publish(jt) marker = False #Robot-to-human handover if(self.action == 'robot'): rospy.sleep(1) jt = JointTrajectory() jt.joint_names = ['head_1_joint', 'head_2_joint'] jtp = JointTrajectoryPoint() jtp.positions = [0, -0.56] jtp.time_from_start = rospy.Duration(1.0) jt.points.append(jtp) self.head_cmd.publish(jt) rospy.sleep(1.0) touch_links=['gripper_link', 'gripper_left_finger_link', 'gripper_right_finger_link', 'gripper_grasping_frame'] #Detect marker while(not marker): print("Action started") try: marker = True #Lookup transform between the base_footprint and arm_goal, which specifies the pregrasp position (trans,rot) = self.listener.lookupTransform('/base_footprint', '/arm_goal', rospy.Time(0)) print("waiting") self.write("Marker location " + str(trans)) self.write("Marker orientation " + str(rot)) #Add object as collision object object_pose = geometry_msgs.msg.PoseStamped() object_pose.header.frame_id = "aruco_marker_frame" object_pose.pose.position.x= -0.1 object_pose.pose.orientation.w=1.0 object_name = "object" self.scene.remove_attached_object(self.eef_link, name=object_name) self.scene.remove_world_object(object_name) self.scene.add_box(object_name, object_pose, size=(self.object[0], self.object[1], self.object[2])) goal = PlayMotionGoal() goal.motion_name = 'open_gripper' goal.skip_planning = False self.play_m_as.send_goal_and_wait(goal) x = self.marker_xtion[0] y = self.marker_xtion[1] #Track object self.look_at(x,y, block=False) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue if not cartesian: #Compute trajectory by specyfying end-effector goal directly pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = rot[0] pose_target.orientation.y = rot[1] pose_target.orientation.z = rot[2] pose_target.orientation.w = rot[3] pose_target.position.x = trans[0] pose_target.position.y = trans[1] pose_target.position.z = trans[2] self.group.set_pose_target(pose_target) self.message="moving" self.pub.publish(self.message) self.write("Starts moving") init_movement = datetime.now() plan_time_start = datetime.now() self.group.set_planner_id("ESTkConfigDefault") plan = self.group.plan() plan_time_start = datetime.now() plan_execute = datetime.now() - plan_time_start print("Time for plan", plan_execute) self.group.go() rospy.sleep(20) self.interpreter.execute("go forward 0.09") print("completed path time", datetime.now() - plan_time_start) else: plan_time_start = datetime.now() if smoothed: #Compute cartesian path planning with smoothing send_data = Path() waypoints = [] wpose = self.group.get_current_pose().pose waypoints.append(copy.deepcopy(wpose)) wpose.position.x = trans[0] wpose.position.y = trans[1] wpose.position.z = trans[2] wpose.orientation.x = rot[0] wpose.orientation.y = rot[1] wpose.orientation.z = rot[2] wpose.orientation.w = rot[3] new_pose = PoseStamped() new_pose.pose = wpose send_data.poses.append(copy.deepcopy(new_pose)) wpose.position.x += 0.1 waypoints.append(copy.deepcopy(wpose)) new_pose = PoseStamped() new_pose.pose = wpose send_data.poses.append(copy.deepcopy(new_pose)) self.message="moving" print self.message self.pub.publish(self.message) self.write("Starts moving") init_movement = datetime.now() while (self.plan_feedback == "Done"): rospy.sleep(0.05) msg = String() msg.data = "request sent" self.plan_done.publish(msg) self.send_waypoints.publish(send_data) print(self.plan_feedback) while (self.plan_feedback!= "Done"): rospy.sleep(0.05) #Compute cartesian path planning without smoothing else: (plan, fraction) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.03, # eef_step 0.0,True) # jump_threshold self.group.execute(plan, wait=True) print("completed path time", datetime.now() - plan_time_start) rospy.sleep(0.5)
def opendoor(req): # main(whole_body, gripper,wrist_wrench) frame = req.handle_pose.header.frame_id hanlde_pos = req.handle_pose.pose # hanlde_pos=geometry_msgs.msg.PoseStamped() res = OpendoorResponse() armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command', JointTrajectory, queue_size=1) robot = hsrb_interface.Robot() whole_body = robot.get('whole_body') gripper = robot.get('gripper') wrist_wrench = robot.get('wrist_wrench') # with hsrb_interface.Robot() as robot: # whole_body = robot.get('whole_body') # gripper = robot.get('gripper') # wrist_wrench = robot.get('wrist_wrench') try: # Open the gripper print("Opening the gripper") whole_body.move_to_neutral() switch = ImpedanceControlSwitch() gripper.command(1.0) # Approach to the door print("Approach to the door") grab_pose = geometry.multiply_tuples( geometry.pose(x=hanlde_pos.position.x - HANDLE_TO_HAND_POS, y=hanlde_pos.position.y, z=hanlde_pos.position.z, ej=math.pi / 2), geometry.pose(ek=math.pi / 2)) whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF) # Close the gripper wrist_wrench.reset() switch.activate("grasping") gripper.grasp(-0.008) rospy.sleep(1.0) switch.inactivate() # Rotate the handle whole_body.impedance_config = 'grasping' 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() # set the desired value current_positions = [ latest_positions[name] for name in traj.joint_names ] current_positions[0] = latest_positions["arm_lift_joint"] - 0.04 current_positions[1] = latest_positions["arm_flex_joint"] - 0.015 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.55 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(5.0) whole_body.impedance_config = 'grasping' whole_body.move_end_effector_by_line((0, 0, 1), 0.35) whole_body.impedance_config = None gripper.command(1.0) whole_body.move_to_neutral() res.success = True except Exception as e: rospy.logerr(e) print "Failed to open door" res.success = False return res
def make_named_trajectory(self, trajectory): """ Makes joint value trajectory from specified by named poses (either from SRDF or from warehouse) @param trajectory - list of waypoints, each waypoint is a dict with the following elements (n.b either name or joint_angles is required) - name -> the name of the way point - joint_angles -> a dict of joint names and angles - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp - degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent. """ current = self.get_current_state_bounded() joint_trajectory = JointTrajectory() joint_names = current.keys() joint_trajectory.joint_names = joint_names start = JointTrajectoryPoint() start.positions = current.values() start.time_from_start = rospy.Duration.from_sec(0.001) joint_trajectory.points.append(start) time_from_start = 0.0 for wp in trajectory: joint_positions = None if 'name' in wp.keys(): joint_positions = self.get_named_target_joint_values( wp['name']) elif 'joint_angles' in wp.keys(): joint_positions = copy.deepcopy(wp['joint_angles']) if 'degrees' in wp.keys() and wp['degrees']: for joint, angle in joint_positions.iteritems(): joint_positions[joint] = radians(angle) if joint_positions is None: rospy.logerr( "Invalid waypoint. Must contain valid name for named target or dict of joint angles." ) return None new_positions = {} for n in joint_names: new_positions[n] = joint_positions[ n] if n in joint_positions else current[n] trajectory_point = JointTrajectoryPoint() trajectory_point.positions = [ new_positions[n] for n in joint_names ] current = new_positions time_from_start += wp['interpolate_time'] trajectory_point.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(trajectory_point) if 'pause_time' in wp and wp['pause_time'] > 0: extra = JointTrajectoryPoint() extra.positions = trajectory_point.positions time_from_start += wp['pause_time'] extra.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(extra) return joint_trajectory
print "waiting to connect..." client.wait_for_server() print "connected! " g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] p1 = JointTrajectoryPoint() p1.positions = [1.5, -0.2, -1.57, 0, 0, 0] p1.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] p1.time_from_start = rospy.Duration(5.0) g.trajectory.points.append(p1) p2 = JointTrajectoryPoint() p2.positions = [1.5, 0, -1.57, 0, 0, 0] p2.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] p2.time_from_start = rospy.Duration(10.0) g.trajectory.points.append(p2) p3 = JointTrajectoryPoint() p3.positions = [2.2, 0, -1.57, 0, 0, 0] p3.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] p3.time_from_start = rospy.Duration(15.0) g.trajectory.points.append(p3) client.send_goal(g)
def set_ee_cartesian_trajectory(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, moving_time=None, wp_moving_time=0.5, wp_accel_time=0.25, wp_period=0.05): if self.resp.num_joints < 6 and (y is not 0 or yaw is not 0): rospy.loginfo( "Please leave the 'y' and 'yaw' fields at '0' when working with arms that have less than 6dof." ) return False rpy = ang.rotationMatrixToEulerAngles(self.T_sb[:3, :3]) T_sy = np.identity(4) T_sy[:3, :3] = ang.eulerAnglesToRotationMatrix([0.0, 0.0, rpy[2]]) T_yb = np.dot(mr.TransInv(T_sy), self.T_sb) rpy[2] = 0.0 if (moving_time == None): moving_time = self.moving_time accel_time = self.accel_time N = int(moving_time / wp_period) inc = 1.0 / float(N) joint_traj = JointTrajectory() joint_positions = list(self.joint_positions) for i in range(N + 1): joint_traj_point = JointTrajectoryPoint() joint_traj_point.positions = joint_positions joint_traj_point.time_from_start = rospy.Duration.from_sec( i * wp_period) joint_traj.points.append(joint_traj_point) if (i == N): break T_yb[:3, 3] += [inc * x, inc * y, inc * z] rpy[0] += inc * roll rpy[1] += inc * pitch rpy[2] += inc * yaw T_yb[:3, :3] = ang.eulerAnglesToRotationMatrix(rpy) T_sd = np.dot(T_sy, T_yb) theta_list, success = self.set_ee_pose_matrix(T_sd, joint_positions, False, blocking=False) if success: joint_positions = theta_list else: rospy.loginfo( "%.1f%% of trajectory successfully planned. Trajectory will not be executed." % (i / float(N) * 100)) break if success: self.set_trajectory_time(wp_moving_time, wp_accel_time) joint_traj.joint_names = self.resp.joint_names[:self.resp. num_joints] with self.js_mutex: current_positions = list( self.joint_states.position[self.waist_index:( self.resp.num_joints + self.waist_index)]) joint_traj.points[0].positions = current_positions joint_traj.header.stamp = rospy.Time.now() self.pub_joint_traj.publish(joint_traj) rospy.sleep(moving_time + wp_moving_time) self.T_sb = T_sd self.joint_positions = joint_positions self.set_trajectory_time(moving_time, accel_time) return success
def main(): os.makedirs( os.path.join(assets_dir(), 'data', '{}_ur3_train_data'.format(args.tag))) rospy.init_node('send_joints') # create a publish node named send_joints pub = rospy.Publisher('/trajectory_controller/command', JointTrajectory, queue_size=10) # Create the topic message traj = JointTrajectory() traj.header = Header() # Joint names for UR traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] rate = rospy.Rate(10) f_joint = open( os.path.join(assets_dir(), 'data/{}_ur3_train_data/joint_data.txt'.format(args.tag)), "a+") f_end_effector = open( os.path.join( assets_dir(), 'data/{}_ur3_train_data/end_effector_data.txt'.format(args.tag)), "a+") iteration = 0 while not rospy.is_shutdown(): traj.header.stamp = rospy.Time.now() pts = JointTrajectoryPoint() random_pos_shoulder_lift_joint = np.random.uniform( -3.14, 0) # set shoulder_lift_joint angle random_pos = np.random.uniform(-3.14, 3.14, size=(5)) # set other joints angle random_pos = np.insert(random_pos, 1, random_pos_shoulder_lift_joint) print("random_pos", random_pos) pts.positions = random_pos pts.time_from_start = rospy.Duration(1.0) # Set the points to the trajectory traj.points = [] traj.points.append(pts) # Publish the message pub.publish(traj) time.sleep(5) # Delays for 5 seconds. x_position, y_position, z_position = pos_main() end_effector_position = np.array([x_position, y_position, z_position]) # pickle store data process...... # pickle.dump(random_pos, open(os.path.join(assets_dir(), # 'data/{}_ur5_train_data/joint_angle_data.pkl'.format(args.tag)), 'wb'), # protocol=pickle.HIGHEST_PROTOCOL) # pickle.dump((x_position, y_position, z_position), open(os.path.join(assets_dir(), # 'data/{}_ur5_train_data/end_effector_position_data.pkl'.format(args.tag)), 'wb'), # protocol=pickle.HIGHEST_PROTOCOL) print >> f_joint, random_pos print >> f_end_effector, end_effector_position iteration += 1 print("iteration", iteration) if iteration == 1000: break
def computeIKsPose(self, poselist, times, arm="arm", downsample_freq=None): """Giving a poselist (list of Pose) and times for every point compute it's iks and add it's times if a value is given to downsample_freq then we will only compute IKs and so for the downsampled rate of poses""" rospy.loginfo("Computing " + str(len(poselist)) + " IKs") fjt_goal = FollowJointTrajectoryGoal() if arm == 'arm_torso': fjt_goal.trajectory.joint_names = self.arm_torso elif arm == 'arm_torso': fjt_goal.trajectory.joint_names = self.arm_torso elif arm == 'arm': fjt_goal.trajectory.joint_names = self.arm elif arm == 'arm': fjt_goal.trajectory.joint_names = self.arm # TODO: add other options ik_answer = None last_succesfull_ik_answer = None if downsample_freq != None: num_poses = 0 num_downsampled_poses = 0 for pose, time in zip(poselist, times): if downsample_freq != None: num_poses += 1 if num_poses % downsample_freq == 0: num_downsampled_poses += 1 else: # if we are downsampling dont do IK calls if it's not one of the samples we want continue if last_succesfull_ik_answer != None: ik_answer = self.getIkPose( pose, "arm", previous_state=last_succesfull_ik_answer.solution) else: ik_answer = self.getIkPose(pose) if DEBUG_MODE: rospy.loginfo("Got error_code: " + str(ik_answer.error_code.val) + " which means: " + moveit_error_dict[ik_answer.error_code.val]) if moveit_error_dict[ik_answer.error_code.val] == 'SUCCESS': # We should check if the solution is very far away from joint config # if so.. try again... being a generic solution i dont know how to manage this last_succesfull_ik_answer = ik_answer if DEBUG_MODE: arrow = self.createArrowMarker(pose, ColorRGBA(0, 1, 0, 1)) self.ok_markers.markers.append(arrow) jtp = JointTrajectoryPoint() #ik_answer = GetConstraintAwarePositionIKResponse() # sort positions and add only the ones of the joints we are interested in positions = self.sortOutJointList( fjt_goal.trajectory.joint_names, ik_answer.solution.joint_state) jtp.positions = positions jtp.time_from_start = rospy.Duration(time) fjt_goal.trajectory.points.append(jtp) if DEBUG_MODE: self.pub_ok_markers.publish(self.ok_markers) else: if DEBUG_MODE: arrow = self.createArrowMarker(pose, ColorRGBA(1, 0, 0, 1)) self.fail_markers.markers.append(arrow) self.pub_fail_markers.publish(self.fail_markers) # Loop for a while to check if we get a solution on further checks? if downsample_freq != None: rospy.loginfo("From " + str(num_poses) + " points we downsampled to " + str(num_downsampled_poses) + " and fjt_goal.trajectory.points has " + str(fjt_goal.trajectory.points) + " points") return fjt_goal
def takeAction(self, action): rospy.wait_for_service('/gazebo/unpause_physics') try: self.unpause() print("debugX") except (rospy.ServiceException) as e: rospy.loginfo("[Walker unpause]: ROS unpause failed!") joint = JointTrajectory() points = JointTrajectoryPoint() nameJoints = [ 'l_shoulder_swing_joint', 'l_shoulder_lateral_joint', 'l_elbow_joint', 'r_shoulder_swing_joint', 'r_shoulder_lateral_joint', 'r_elbow_joint', 'l_hip_twist_joint', 'l_hip_lateral_joint', 'l_hip_swing_joint', 'l_knee_joint', 'l_ankle_swing_joint', 'l_ankle_lateral_joint', 'r_hip_twist_joint', 'r_hip_lateral_joint', 'r_hip_swing_joint', 'r_knee_joint', 'r_ankle_swing_joint', 'r_ankle_lateral_joint' ] newAction = [ 0, 0, 0, 0, 0, 0, 0, 0, action[0], action[1], action[2], 0, 0, 0, action[2], action[3], action[4], 0 ] points.positions = newAction points.time_from_start = rospy.Duration(0.01, 0) joint.points = [points] joint.joint_names = nameJoints joint.header.stamp = rospy.Time.now() self.pubJoint.publish(joint) print("debugY") reward = -0.1 currentTime = time.time() if ((self.robotState.odometryX - self.robotState.lastOdometryX) >= 0.1) or ((self.robotState.odometryX - self.robotState.lastOdometryX) < 0): #-0.001forward deltaTime = currentTime - self.robotState.lastTime reward += ((self.robotState.odometryX - self.robotState.lastOdometryX)) * 30 print("[Ganho]:", reward) print("[Distancia]:", self.robotState.odometryX - self.robotState.lastOdometryX) self.robotState.lastTime = currentTime self.robotState.lastOdometryX = self.robotState.odometryX if (self.robotState.contactFootR > 20 and self.robotState.contactFootL > 20 and self.robotState.odomPosZ <= 0.18): reward += -100 self.robotState.done = True self.robotState.fall = 1 if (np.abs(self.robotState.odomRotation.x) > 0.2 or np.abs(self.robotState.odomRotation.y) > 0.2 or np.abs(self.robotState.odomRotation.z) > 0.2): reward += -80 self.robotState.done = True self.robotState.fall = 1 print("debugB") if self.robotState.odometryX > 9.0: reward += 100 self.robotState.done = True self.robotState.fall = 1 print("\33[92mREACHED TO THE END!\33[0m") self.rate.sleep() return reward, self.robotState.done
def on_enter(self, userdata): self._param_error = False self._request_failed = False self._moveit_failed = False self._success = False # Parameter check if self._srdf_param is None: self._param_error = True return try: self._srdf = ET.fromstring(self._srdf_param) except Exception as e: Logger.logwarn( 'Unable to parse given SRDF parameter: /robot_description_semantic' ) self._param_error = True if not self._param_error: robot = None for r in self._srdf.iter('robot'): if self._robot_name == '' or self._robot_name == r.attrib[ 'name']: robot = r break if robot is None: Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name) return 'param_error' config = None for c in robot.iter('group_state'): if (self._move_group == '' or self._move_group == c.attrib['group']) \ and c.attrib['name'] == self._config_name: config = c break if config is None: Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name) return 'param_error' try: self._joint_config = [ float(j.attrib['value']) for j in config.iter('joint') ] self._joint_names = [ str(j.attrib['name']) for j in config.iter('joint') ] except Exception as e: Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e)) return 'param_error' # Action Initialization action_goal = ExecuteKnownTrajectoryRequest( ) # ExecuteTrajectoryGoal() action_goal.trajectory.joint_trajectory.header.stamp = rospy.Time.now( ) + rospy.Duration(0.2) action_goal.trajectory.joint_trajectory.joint_names = [''] * len( self._joint_names) joint_trajectory_point = JointTrajectoryPoint() action_goal.trajectory.joint_trajectory.points = [ joint_trajectory_point ] joint_trajectory_point.positions = [0.0] * len(self._joint_names) joint_trajectory_point.time_from_start = rospy.Duration( self._duration) action_goal.wait_for_execution = True for i in range(len(self._joint_names)): action_goal.trajectory.joint_trajectory.joint_names[ i] = self._joint_names[i] action_goal.trajectory.joint_trajectory.points[0].positions[ i] = self._joint_config[i] try: self._response = self._client.call(self._action_topic, action_goal) Logger.loginfo( "Execute Known Trajectory Service requested: \n" + str(self._action_topic) + " " + str(action_goal)) except rospy.ServiceException as exc: Logger.logwarn( "Execute Known Trajectory Service did not process request: \n" + str(exc)) self._request_failed = True return
import actionlib import math from control_msgs.msg import FollowJointTrajectoryAction client = actionlib.SimpleActionClient('/panda_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) client.wait_for_server() from control_msgs.msg import FollowJointTrajectoryGoal goal = FollowJointTrajectoryGoal() from trajectory_msgs.msg import JointTrajectory joint_traj = JointTrajectory() joint_traj.joint_names = ["panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7"] from trajectory_msgs.msg import JointTrajectoryPoint point1 = JointTrajectoryPoint() point2 = JointTrajectoryPoint() point1.positions = [0.33877080806309046, -0.3623406480725775, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795, 0.06015034910227879] point1.velocities = [0., 0., 0., 0., 0., 0., 0.] #point2.positions = [0.33877080806309046, -0.3623406480725775, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795 - math.pi/2, 0.06015034910227879] point2.positions = [0.33877080806309046, -1.5, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795 - math.pi/2, 0.06015034910227879] point2.velocities = [0., 0., 0., 0., 0., 0., 0.] point1.time_from_start = rospy.Duration(2.) point2.time_from_start = rospy.Duration(4.) from copy import deepcopy joint_traj.points = [point1, point2, deepcopy(point1), deepcopy(point2)] joint_traj.points[2].time_from_start += rospy.Duration(4.) joint_traj.points[3].time_from_start += rospy.Duration(4.) goal.trajectory = joint_traj joint_traj.header.stamp = rospy.Time.now()+rospy.Duration(1.0) client.send_goal_and_wait(goal)
def add_point(self, positions, time): point = JointTrajectoryPoint() point.positions = copy(positions) point.velocities = [0.0] * len(self._goal.trajectory.joint_names) point.time_from_start = rospy.Duration(time) self._goal.trajectory.points.append(point)
def add_point(self, positions, time): point = JointTrajectoryPoint() point.positions = copy(positions) point.time_from_start = rospy.Duration(time) self._goal.trajectory.points.append(point)
def create_grasp(self, pose, grasp_id): """ :type pose: Pose pose of the gripper for the grasp :type grasp_id: str name for the grasp :rtype: Grasp """ g = Grasp() g.id = grasp_id pre_grasp_posture = JointTrajectory() pre_grasp_posture.header.frame_id = self._grasp_postures_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) grasp_posture = copy.deepcopy(pre_grasp_posture) grasp_posture.points[0].time_from_start = rospy.Duration( self._time_pre_grasp_posture + self._time_grasp_posture) jtpoint2 = JointTrajectoryPoint() jtpoint2.positions = [ float(pos) for pos in self._gripper_grasp_positions.split()] jtpoint2.time_from_start = rospy.Duration( self._time_pre_grasp_posture + self._time_grasp_posture + self._time_grasp_posture_final) grasp_posture.points.append(jtpoint2) g.pre_grasp_posture = pre_grasp_posture g.grasp_posture = grasp_posture header = Header() header.frame_id = self._grasp_pose_frame_id # base_footprint q = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] # Fix orientation from gripper_link to parent_link (tool_link) fix_tool_to_gripper_rotation_q = quaternion_from_euler( math.radians(self._fix_tool_frame_to_grasping_frame_roll), math.radians(self._fix_tool_frame_to_grasping_frame_pitch), math.radians(self._fix_tool_frame_to_grasping_frame_yaw) ) q = quaternion_multiply(q, fix_tool_to_gripper_rotation_q) fixed_pose = copy.deepcopy(pose) fixed_pose.orientation = Quaternion(*q) g.grasp_pose = PoseStamped(header, fixed_pose) g.grasp_quality = self._grasp_quality g.pre_grasp_approach = GripperTranslation() g.pre_grasp_approach.direction.vector.x = self._pre_grasp_direction_x # NOQA g.pre_grasp_approach.direction.vector.y = self._pre_grasp_direction_y # NOQA g.pre_grasp_approach.direction.vector.z = self._pre_grasp_direction_z # NOQA g.pre_grasp_approach.direction.header.frame_id = self._grasp_postures_frame_id # NOQA g.pre_grasp_approach.desired_distance = self._grasp_desired_distance # NOQA g.pre_grasp_approach.min_distance = self._grasp_min_distance g.post_grasp_retreat = GripperTranslation() g.post_grasp_retreat.direction.vector.x = self._post_grasp_direction_x # NOQA g.post_grasp_retreat.direction.vector.y = self._post_grasp_direction_y # NOQA g.post_grasp_retreat.direction.vector.z = self._post_grasp_direction_z # NOQA g.post_grasp_retreat.direction.header.frame_id = self._grasp_postures_frame_id # NOQA g.post_grasp_retreat.desired_distance = self._grasp_desired_distance # NOQA g.post_grasp_retreat.min_distance = self._grasp_min_distance g.max_contact_force = self._max_contact_force g.allowed_touch_objects = self._allowed_touch_objects return g
def functional(axis, angle): pub_rosey = rospy.Publisher( '/cyton_joint_trajectory_action_controller/command', JointTrajectory, queue_size=10) rospy.init_node('traj_maker', anonymous=True) time.sleep(1) axis = int(axis) angle = float(angle) rate = rospy.Rate(0.01) while not rospy.is_shutdown(): # first way to define a point traj_waypoint_1_rosey = JointTrajectoryPoint() traj_waypoint_1_rosey.positions = [0,0,0,0,0,0,0] traj_waypoint_1_rosey.time_from_start = Duration(2) # second way to define a point traj_waypoint_2_rosey = traj_waypoint_1_rosey traj_waypoint_2_rosey.time_from_start = Duration(4) traj_waypoint_2_rosey.positions[axis-1] = angle time.sleep(1) traj_waypoint_3_rosey = traj_waypoint_2_rosey traj_waypoint_3_rosey.time_from_start = Duration(7) #traj_waypoint_2_rosey = JointTrajectoryPoint(positions=[.31, -.051, .33, -.55, .28, .60,0], #time_from_start = Duration(4)) #traj_waypoint_3_rosey = JointTrajectoryPoint(positions=[.14726, -.014151, .166507, -.33571, .395997, .38657,0], #time_from_start = Duration(6)) #traj_waypoint_4_rosey = JointTrajectoryPoint(positions=[-.09309, .003150, .003559, .16149, .524427, -.1867,0], #time_from_start = Duration(8)) #traj_waypoint_5_rosey = JointTrajectoryPoint(positions=[-.27752, .077886, -.1828, .38563, .682589, -.44665,0], #time_from_start = Duration(10)) #traj_waypoint_6_rosey = JointTrajectoryPoint(positions=[0,0,0,0,0,0,0],time_from_start = Duration(12)) #debug in terminal print traj_waypoint_2_rosey.positions # making message message_rosey = JointTrajectory() # required headers header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now()) message_rosey.header = header_rosey # adding in joints joint_names = ['shoulder_roll_joint', \ 'shoulder_pitch_joint', 'shoulder_yaw_joint', 'elbow_pitch_joint', \ 'elbow_yaw_joint', 'wrist_pitch_joint', 'wrist_roll_joint'] message_rosey.joint_names = joint_names # appending up to 100 points # ex. for i in enumerate(len(waypoints)): append(waypoints[i]) message_rosey.points.append(traj_waypoint_1_rosey) message_rosey.points.append(traj_waypoint_2_rosey) message_rosey.points.append(traj_waypoint_3_rosey) #message_rosey.points.append(traj_waypoint_4_rosey) #message_rosey.points.append(traj_waypoint_5_rosey) #message_rosey.points.append(traj_waypoint_6_rosey) # publishing to ROS node pub_rosey.publish(message_rosey) rate.sleep() if rospy.is_shutdown(): break
def construct_trajectory_point(posture, duration): trajectory_point = JointTrajectoryPoint() trajectory_point.time_from_start = rospy.Duration.from_sec(float(duration)) for key in joint_trajectory.joint_names: trajectory_point.positions.append(posture[key]) return trajectory_point
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint rospy.init_node('send_open_manipulator_arm_joint_angles') pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=1) controller_name = "arm_controller" joint_names = rospy.get_param("/%s/joints" % controller_name) rospy.loginfo("Joint names: %s" % joint_names) rate = rospy.Rate(10) trajectory_command = JointTrajectory() trajectory_command.joint_names = joint_names point = JointTrajectoryPoint() #Joint names: ['joint1', 'joint2', 'joint3', 'joint4'] point.positions = [-0.6, 0.6, 0.1, -0.7] point.velocities = [0.0, 0.0, 0.0, 0.0] point.time_from_start = rospy.rostime.Duration(1, 0) trajectory_command.points = [point] while not rospy.is_shutdown(): trajectory_command.header.stamp = rospy.Time.now() pub.publish(trajectory_command) rate.sleep()
def generate_grasps(self, name, pose): grasps = [] now = rospy.Time.now() #for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(10.0)): # Create place location: angle = 0 grasp = Grasp() grasp.grasp_pose.header.stamp = now grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame() grasp.grasp_pose.pose = copy.deepcopy(pose) # Setting pre-grasp approach grasp.pre_grasp_approach.direction.header.stamp = now grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame( ) grasp.pre_grasp_approach.direction.vector.z = -0.2 grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist # Setting post-grasp retreat grasp.post_grasp_retreat.direction.header.stamp = now grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame( ) grasp.post_grasp_retreat.direction.vector.z = 0.2 grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist # if name != "cylinder": q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle) grasp.grasp_pose.pose.orientation = Quaternion(*q) # else: # #grasp.pre_grasp_approach.direction.vector.z = -0.05 # grasp.pre_grasp_approach.direction.vector.x = 0.1 # #grasp.post_grasp_retreat.direction.vector.z = 0.05 # grasp.post_grasp_retreat.direction.vector.x = -0.1 grasp.max_contact_force = 100 grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() traj.positions.append(0.03) traj.time_from_start = rospy.Duration.from_sec(0.5) grasp.pre_grasp_posture.points.append(traj) grasp.grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() if name == "box": traj.positions.append(0.57) elif name == "sphere": traj.positions.append(0.3) else: traj.positions.append(0.3) #traj.velocities.append(0.2) traj.effort.append(800) traj.time_from_start = rospy.Duration.from_sec(5.0) grasp.grasp_posture.points.append(traj) grasps.append(grasp) return grasps
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 #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 start_pose_stamped = self.ik_utils.run_fk(reordered_start_angles, self.ik_utils.link_name) #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 if max( error_codes) == 0 else ArmNavigationErrorCodes.PLANNING_FAILED # 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 publishAngles(self): traj = JointTrajectory() point = JointTrajectoryPoint() traj.header.frame_id = "/" # leg1 traj.joint_names.append("lf_hip_joint") traj.joint_names.append("lf_upper_leg_joint") traj.joint_names.append("lf_lower_leg_joint") # point.positions.append(self.leg1_angles[0]) # point.positions.append(self.leg1_angles[1] - self.leg1_angles[2]) # point.positions.append(self.leg1_angles[3]) point.positions.append(self.leg1_angles[0]) point.positions.append((self.leg1_angles[1] + self.leg1_angles[2])) point.positions.append(self.leg1_angles[3]) # leg2 traj.joint_names.append("rf_hip_joint") traj.joint_names.append("rf_upper_leg_joint") traj.joint_names.append("rf_lower_leg_joint") # point.positions.append(self.leg2_angles[0]) # point.positions.append(self.leg2_angles[1] - self.leg2_angles[2]) # point.positions.append(self.leg2_angles[3]) point.positions.append(self.leg2_angles[0]) point.positions.append((self.leg2_angles[1] + self.leg2_angles[2])) point.positions.append(self.leg2_angles[3]) # leg3 traj.joint_names.append("rh_hip_joint") traj.joint_names.append("rh_upper_leg_joint") traj.joint_names.append("rh_lower_leg_joint") # point.positions.append(self.leg3_angles[0]) # point.positions.append(self.leg3_angles[1] - self.leg3_angles[2]) # point.positions.append(self.leg3_angles[3]) point.positions.append(self.leg3_angles[0]) point.positions.append((self.leg3_angles[1] + self.leg3_angles[2])) point.positions.append(self.leg3_angles[3]) # leg4 traj.joint_names.append("lh_hip_joint") traj.joint_names.append("lh_upper_leg_joint") traj.joint_names.append("lh_lower_leg_joint") # point.positions.append(self.leg4_angles[0]) # point.positions.append(self.leg4_angles[1] - self.leg4_angles[2]) # point.positions.append(self.leg4_angles[3]) point.positions.append(self.leg4_angles[0]) point.positions.append((self.leg4_angles[1] + self.leg4_angles[2])) point.positions.append(self.leg4_angles[3]) for i in range(12): point.velocities.append(0) point.accelerations.append(0) # point.positions.append(0) point.time_from_start = rospy.Duration(0, 2e8) traj.points.append(point) traj.header.stamp = rospy.Time.now() # print(traj) self.pub.publish(traj) self.rate.sleep()
names = ['motor_one', 'motor_two'] if __name__ == '__main__': rospy.init_node('pololu_action_example_client') client = actionlib.SimpleActionClient('pololu_trajectory_action_server', pololu_trajectoryAction) client.wait_for_server() goal = pololu_trajectoryGoal() traj = goal.joint_trajectory traj.header.stamp = rospy.Time.now() traj.joint_names.append(names[0]) traj.joint_names.append(names[1]) pts = JointTrajectoryPoint() pts.time_from_start = rospy.Duration(2.0) pts.positions.append(-0.77) pts.positions.append(0.77) pts.velocities.append(1.0) pts.velocities.append(1.0) traj.points.append(pts) pts = JointTrajectoryPoint() pts.time_from_start = rospy.Duration(3.0) pts.positions.append(0.77) pts.positions.append(-0.77) pts.velocities.append(1.0) pts.velocities.append(1.0) traj.points.append(pts) # Fill in the goal here client.send_goal(goal)
def _on_trajectory_action(self, goal): joint_names = goal.trajectory.joint_names self._get_trajectory_parameters(joint_names, goal) num_joints = len(joint_names) trajectory_points = goal.trajectory.points pnt_times = [0.0] * len(trajectory_points) # Create a new discretized joint trajectory num_points = len(trajectory_points) if num_points == 0: rospy.logerr("%s: Empty Trajectory" % (action_name, )) self._server.set_aborted() return # Force Velocites/Accelerations to zero at the final timestep # if they exist in the trajectory # Remove this behavior if you are stringing together trajectories, # and want continuous, non-zero velocities/accelerations between # trajectories trajectory_points[-1].velocities = [0.0] * len(joint_names) trajectory_points[-1].accelerations = [0.0] * len(joint_names) # Compute Full Bezier Curve Coefficients for all 7 joints pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points] if (num_points == 1) or (pnt_times[0] > 0.0): # Add current position as trajectory point first_trajectory_point = JointTrajectoryPoint() first_trajectory_point.positions = self._get_current_position( joint_names) # To preserve desired velocities and accelerations, copy them to the first # trajectory point if the trajectory is only 1 point. first_trajectory_point.time_from_start = rospy.Duration(0) trajectory_points.insert(0, first_trajectory_point) num_points = len(trajectory_points) for i in range(num_points): trajectory_points[i].velocities = [0.0] * len(joint_names) trajectory_points[i].accelerations = [0.0] * len(joint_names) for i in range(1, num_points): for j in range(num_joints): if ((pnt_times[i] - pnt_times[i - 1]) > 0.0): trajectory_points[i].velocities[j] = ( trajectory_points[i].positions[j] - trajectory_points[i - 1].positions[j]) / ( pnt_times[i] - pnt_times[i - 1]) """ Wait for the specified execution time, if not provided use now """ start_time = goal.trajectory.header.stamp.to_sec() now = rospy.get_time() if start_time == 0.0: start_time = rospy.get_time() while start_time > now: now = rospy.get_time() """ Loop until end of trajectory time. Provide a single time step of the control rate past the end to ensure we get to the end. Keep track of current indices for spline segment generation """ control_rate = rospy.Rate(self._trajectory_control_rate) now_from_start = rospy.get_time() - start_time end_time = trajectory_points[-1].time_from_start.to_sec() last_idx = 0 slewed_pos = self._get_current_position(joint_names) self._command_joints(joint_names, deepcopy(trajectory_points[0])) while (now_from_start < end_time and not rospy.is_shutdown() and self.robot_is_enabled()): now = rospy.get_time() now_from_start = now - start_time for i in range(num_points): if (pnt_times[i] >= now_from_start): idx = i break if (idx >= num_points): idx = num_points - 1 for j in range(num_joints): slewed_pos[j] += trajectory_points[idx].velocities[j] * ( 1.0 / self._trajectory_control_rate) point = deepcopy(trajectory_points[idx]) point.positions = slewed_pos """ Command Joint Position, Velocity, Acceleration """ command_executed = self._command_joints(joint_names, point) self._update_feedback(deepcopy(point), joint_names, now_from_start) """ Break the loop if the command cannot be executed """ if not command_executed: return control_rate.sleep() """ Keep trying to meet goal until goal_time constraint expired """ last = trajectory_points[-1] last_time = trajectory_points[-1].time_from_start.to_sec() end_angles = dict(zip(joint_names, last.positions)) while (now_from_start < (last_time + self._goal_time) and not rospy.is_shutdown() and self.robot_is_enabled()): if not self._command_joints(joint_names, last): return now_from_start = rospy.get_time() - start_time self._update_feedback(deepcopy(last), joint_names, now_from_start) control_rate.sleep() now_from_start = rospy.get_time() - start_time self._update_feedback(deepcopy(last), joint_names, now_from_start) """ Verify goal constraint """ result = self._check_goal_state(joint_names, last) if result is True: rospy.loginfo("%s: Joint Trajectory Action Succeeded" % self._action_name) self._result.error_code = self._result.SUCCESSFUL self._server.set_succeeded(self._result) elif result is False: rospy.logerr("%s: Exceeded Max Goal Velocity Threshold" % self._action_name) self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED self._server.set_aborted(self._result) else: rospy.logerr("%s: Exceeded Goal Threshold Error %s" % (self._action_name, result)) self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED self._server.set_aborted(self._result) self._command_stop()
goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [ 'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint', 'arm_7_joint' ] jtp = JointTrajectoryPoint() data = rospy.get_param("mymotions") points = data.get('play_motion').get('motions').get('grasp17').get( 'points')[0] jtp.positions = points.get('positions') jtp.velocities = [0.12, 0.12, 0.12, 0.12, 0.12, 0.12, 0.12] jtp.time_from_start = rospy.Duration(1.5) #I can set the duration here goal.trajectory.points.append(jtp) client.send_goal(goal) client.wait_for_result() pub.publish('not moving') elif confirmcheck == '2': message = 'stop' stop_pub.publish(message) elif confirmcheck == '3': message = 'go' stop_pub.publish(message) elif confirmcheck == '4':
# a publisher for arm movement client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction) client.wait_for_server() # prepare a joint trajectory msg = JointTrajectory() msg.joint_names = servos msg.points = list() # tucking or untucking? if len(sys.argv) > 1 and sys.argv[1] == "u": point = JointTrajectoryPoint() point.positions = utcked point.velocities = [ 0.0 for servo in msg.joint_names ] point.time_from_start = rospy.Duration(3.0) msg.points.append(point) else: point = JointTrajectoryPoint() point.positions = utcked 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 = tucked point.velocities = [ 0.0 for servo in msg.joint_names ] point.time_from_start = rospy.Duration(8.0) msg.points.append(point) # execute msg.header.stamp = rospy.Time.now()
traj.joint_names = [ 'joint_b', 'joint_s', 'joint_t', 'joint_r', 'joint_u', 'joint_l' ] angle_range = 0.2 time_range = 10 # create a list of JointTrajectoryPoint objects points = [] N = 100 # number of points for i in xrange(N): point = JointTrajectoryPoint() positions = [] velocities = [] time_from_start = rospy.Duration.from_sec(time_range * (i + 1) / float(N)) point.time_from_start = time_from_start position = angle_range * math.sin(2 * math.pi * (i + 1) / float(N)) velocity = 2 * math.pi / float(time_range) * angle_range * math.cos( 2 * math.pi * (i + 1) / float(N)) for j in xrange(6): positions.append(position) velocities.append(velocity) point.positions = positions point.velocities = velocities points.append(point) traj.points = points # rospy.loginfo(traj) trajPub.publish(traj) rospy.loginfo('published?')
def controller(ps, motorOn, control_info): #Controller for creating the command message to pass to the arm_controller # motorOn [in] = specifier for whether to actuate the system (convenience feature for testing) # control_info [in]= control structure specifying the relevant control parameters start_time = rospy.Time.now() time_from_start = 0. ps.final_time = control_info['tf'] if ps.startBag: ps.bagging = True #MinJerk trajectory type specifies waypoints to move to with zero velocity and accelration #boundary conditions if control_info['type'] == "MinJerk": waypoints = np.array( [[-0.02, -0.02, -0.02, 0.365, 0.365, 0.365, 0.0, 0.0], [0.49, 0.49, 0.49, 0.2175, 0.2175, 0.2175, 0.395, 0.395], [0.1, -0.02, 0.1, 0.1, -0.02, 0.1, 0.1, 0.1], [0., 0.0, 0., np.pi / 2, np.pi / 2, 0.0, 0.0, 0.0]]) #Specify time interval for each segment time_array = np.array([2., 2., 2., 2., 2., 2., 2., 2.]) #Compute the total time needed tf = np.sum(time_array) #Use the inverseKinematics function to generate the starting joint configuration initial_angles = TG.inverseKinematics(0.0, 0.395, 0.1, 0) #Compute the minimum jerk constants joint_const = TG.minJerkSetup_now(initial_angles, tf, waypoints, t_array=time_array) #Loop for the duration of the control interval while control_info['tf'] - time_from_start > 0: deflection = ps.queue.deflection[:, -1] temperature = ps.queue.windingTempFlt[:, -1] cmd = model_learning.msg.CommandML() trajectory = JointTrajectory() point = JointTrajectoryPoint() trajectory.header.stamp = rospy.Time.now() point.time_from_start = trajectory.header.stamp - start_time time_from_start = (point.time_from_start.secs + point.time_from_start.nsecs / 1000000000.) #Compute the next time step for the minimum jerk trajectory if control_info['type'] == "MinJerk": pos_v, vel_v, accel_v = TG.minJerkStep_now(time_from_start, tf, waypoints, joint_const, t_array=time_array) #For each of the joints, populate the command trajectory for joint in range(0, 5): if joint == 0 or joint == 1 or joint == 2 or joint == 3 or joint == 4: if control_info['type'] == "Circle": pos_v, vel_v, accel_v = TG.generateJointTrajectory_now( time_from_start) pos = pos_v[joint] vel = vel_v[joint] accel = accel_v[joint] elif control_info['type'] == "MinJerk": pos = pos_v[joint] vel = vel_v[joint] accel = accel_v[joint] else: pos, vel, accel = 0., 0., 0. point.positions.append(pos) point.velocities.append(vel) point.accelerations.append(accel) first_joint = 0 eps = [0., 0., 0., 0., 0.] #Popuylate the command message with appropriate values cmd.epsTau = eps cmd.jointTrajectory.points.append(point) cmd.motorOn = float(motorOn) cmd.controlType = 1. # Designates torque control cmd.closedLoop = control_info['closedLoop'] cmd.feedforward = control_info['feedforward'] cmd.pos_gain = control_info['p_gain'] cmd.vel_gain = control_info['v_gain'] ps.traj_pub.publish(cmd) if ps.startBag: ps.startBag = False ps.bagging = False time.sleep(0.001) ps.bag.close()
def joint_angle_client(): #inhibitWalkSrv = rospy.ServiceProxy("inhibit_walk", std_srvs.srv.Empty) #uninhibitWalkSrv = rospy.ServiceProxy("uninhibit_walk", std_srvs.srv.Empty) client = actionlib.SimpleActionClient("joint_trajectory", naoqi_bridge_msgs.msg.JointTrajectoryAction) stiffness_client = actionlib.SimpleActionClient("joint_stiffness_trajectory", naoqi_bridge_msgs.msg.JointTrajectoryAction) angle_client = actionlib.SimpleActionClient("joint_angles_action", naoqi_bridge_msgs.msg.JointAnglesWithSpeedAction) rospy.loginfo("Waiting for joint_trajectory and joint_stiffness servers...") client.wait_for_server() stiffness_client.wait_for_server() angle_client.wait_for_server() rospy.loginfo("Done.") #inhibitWalkSrv() try: goal = naoqi_bridge_msgs.msg.JointTrajectoryGoal() # move head: single joint, multiple keypoints goal.trajectory.joint_names = ["HeadYaw"] goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.0), positions = [1.0])) goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(2.0), positions = [-1.0])) goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(2.5), positions = [0.0])) rospy.loginfo("Sending goal...") client.send_goal(goal) client.wait_for_result() result = client.get_result() rospy.loginfo("Results: %s", str(result.goal_position.position)) # Test for preemption rospy.loginfo("Sending goal again...") client.send_goal(goal) rospy.sleep(0.5) rospy.loginfo("Preempting goal...") client.cancel_goal() client.wait_for_result() if client.get_state() != GoalStatus.PREEMPTED or client.get_result() == result: rospy.logwarn("Preemption does not seem to be working") else: rospy.loginfo("Preemption seems okay") # crouching pose: single keypoint, multiple joints: goal.trajectory.joint_names = ["Body"] point = JointTrajectoryPoint() point.time_from_start = Duration(1.5) point.positions = [0.0,0.0, # head 1.545, 0.33, -1.57, -0.486, 0.0, 0.0, # left arm -0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # left leg -0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # right leg 1.545, -0.33, 1.57, 0.486, 0.0, 0.0] # right arm goal.trajectory.points = [point] rospy.loginfo("Sending goal...") client.send_goal(goal) client.wait_for_result() rospy.loginfo("Getting results...") result = client.get_result() print "Result:", ', '.join([str(n) for n in result.goal_position.position]) # multiple joints, multiple keypoints: goal.trajectory.joint_names = ["HeadYaw", "HeadPitch"] goal.trajectory.points = [] goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [1.0, 1.0])) goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.0), positions = [1.0, 0.0])) goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.5), positions = [0.0, 0.0])) rospy.loginfo("Sending goal...") client.send_goal(goal) client.wait_for_result() rospy.loginfo("Getting results...") result = client.get_result() print "Result:", ', '.join([str(n) for n in result.goal_position.position]) # multiple joints, single keypoint: goal.trajectory.joint_names = ["HeadYaw", "HeadPitch"] goal.trajectory.points = [] goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [0.5, 0.5])) rospy.loginfo("Sending goal...") client.send_goal(goal) client.wait_for_result() rospy.loginfo("Getting results...") result = client.get_result() print "Result:", ', '.join([str(n) for n in result.goal_position.position]) # Control of joints with relative speed angle_goal = naoqi_bridge_msgs.msg.JointAnglesWithSpeedGoal() angle_goal.joint_angles.relative = False angle_goal.joint_angles.joint_names = ["HeadYaw", "HeadPitch"] angle_goal.joint_angles.joint_angles = [1.0, 0.0] angle_goal.joint_angles.speed = 0.2 rospy.loginfo("Sending joint angles goal...") angle_client.send_goal_and_wait(angle_goal) result = angle_client.get_result() rospy.loginfo("Angle results: %s", str(result.goal_position.position)) # Test for preemption angle_goal.joint_angles.joint_angles = [-1.0, 0.0] angle_goal.joint_angles.speed = 0.05 rospy.loginfo("Sending goal again...") angle_client.send_goal(angle_goal) rospy.sleep(0.5) rospy.loginfo("Preempting goal...") angle_client.cancel_goal() angle_client.wait_for_result() if angle_client.get_state() != GoalStatus.PREEMPTED or angle_client.get_result() == result: rospy.logwarn("Preemption does not seem to be working") else: rospy.loginfo("Preemption seems okay") # Test stiffness actionlib stiffness_goal = naoqi_bridge_msgs.msg.JointTrajectoryGoal() stiffness_goal.trajectory.joint_names = ["Body"] stiffness_goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [1.0])) rospy.loginfo("Sending stiffness goal...") stiffness_client.send_goal(stiffness_goal) stiffness_client.wait_for_result() result = stiffness_client.get_result() rospy.loginfo("Stiffness results: %s", str(result.goal_position.position)) # Test for preemption stiffness_goal.trajectory.points = [JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [0.0])] rospy.loginfo("Sending goal again...") stiffness_client.send_goal(stiffness_goal) rospy.sleep(0.25) rospy.loginfo("Preempting goal...") stiffness_client.cancel_goal() stiffness_client.wait_for_result() if stiffness_client.get_state() != GoalStatus.PREEMPTED or stiffness_client.get_result() == result: rospy.logwarn("Preemption does not seem to be working") else: rospy.loginfo("Preemption seems okay") finally: pass #uninhibitWalkSrv()
def _on_trajectory_action(self, goal): joint_names = goal.trajectory.joint_names trajectory_points = goal.trajectory.points # Load parameters for trajectory self._get_trajectory_parameters(joint_names, goal) # Create a new discretized joint trajectory num_points = len(trajectory_points) if num_points == 0: rospy.logerr("%s: Empty Trajectory" % (self._action_name,)) self._server.set_aborted() return rospy.loginfo("%s: Executing requested joint trajectory" % (self._action_name,)) rospy.logdebug("Trajectory Points: {0}".format(trajectory_points)) control_rate = rospy.Rate(self._control_rate) dimensions_dict = self._determine_dimensions(trajectory_points) if num_points == 1: # Add current position as trajectory point first_trajectory_point = JointTrajectoryPoint() first_trajectory_point.positions = self._get_current_position(joint_names) # To preserve desired velocities and accelerations, copy them to the first # trajectory point if the trajectory is only 1 point. if dimensions_dict['velocities']: first_trajectory_point.velocities = deepcopy(trajectory_points[0].velocities) if dimensions_dict['accelerations']: first_trajectory_point.accelerations = deepcopy(trajectory_points[0].accelerations) first_trajectory_point.time_from_start = rospy.Duration(0) trajectory_points.insert(0, first_trajectory_point) num_points = len(trajectory_points) # Force Velocites/Accelerations to zero at the final timestep # if they exist in the trajectory # Remove this behavior if you are stringing together trajectories, # and want continuous, non-zero velocities/accelerations between # trajectories if dimensions_dict['velocities']: trajectory_points[-1].velocities = [0.0] * len(joint_names) if dimensions_dict['accelerations']: trajectory_points[-1].accelerations = [0.0] * len(joint_names) # Compute Full Bezier Curve Coefficients for all 7 joints pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points] try: b_matrix = self._compute_bezier_coeff(joint_names, trajectory_points, dimensions_dict) except Exception as ex: rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}" " arm with error \"{2}: {3}\"").format( self._action_name, self._name, type(ex).__name__, ex)) self._server.set_aborted() return # Wait for the specified execution time, if not provided use now start_time = goal.trajectory.header.stamp.to_sec() if start_time == 0.0: start_time = rospy.get_time() baxter_dataflow.wait_for( lambda: rospy.get_time() >= start_time, timeout=float('inf') ) # Loop until end of trajectory time. Provide a single time step # of the control rate past the end to ensure we get to the end. # Keep track of current indices for spline segment generation now_from_start = rospy.get_time() - start_time end_time = trajectory_points[-1].time_from_start.to_sec() while (now_from_start < end_time and not rospy.is_shutdown() and self.robot_is_enabled()): #Acquire Mutex now = rospy.get_time() now_from_start = now - start_time idx = bisect.bisect(pnt_times, now_from_start) #Calculate percentage of time passed in this interval if idx >= num_points: cmd_time = now_from_start - pnt_times[-1] t = 1.0 elif idx >= 0: cmd_time = (now_from_start - pnt_times[idx-1]) t = cmd_time / (pnt_times[idx] - pnt_times[idx-1]) else: cmd_time = 0 t = 0 point = self._get_bezier_point(b_matrix, idx, t, cmd_time, dimensions_dict) # Command Joint Position, Velocity, Acceleration command_executed = self._command_joints(joint_names, point, start_time, dimensions_dict) self._update_feedback(deepcopy(point), joint_names, now_from_start) # Release the Mutex if not command_executed: return control_rate.sleep() # Keep trying to meet goal until goal_time constraint expired last = trajectory_points[-1] last_time = trajectory_points[-1].time_from_start.to_sec() end_angles = dict(zip(joint_names, last.positions)) def check_goal_state(): for error in self._get_current_error(joint_names, last.positions): if (self._goal_error[error[0]] > 0 and self._goal_error[error[0]] < math.fabs(error[1])): return error[0] if (self._stopped_velocity > 0.0 and max([abs(cur_vel) for cur_vel in self._get_current_velocities(joint_names)]) > self._stopped_velocity): return False else: return True while (now_from_start < (last_time + self._goal_time) and not rospy.is_shutdown() and self.robot_is_enabled()): if not self._command_joints(joint_names, last, start_time, dimensions_dict): return now_from_start = rospy.get_time() - start_time self._update_feedback(deepcopy(last), joint_names, now_from_start) control_rate.sleep() now_from_start = rospy.get_time() - start_time self._update_feedback(deepcopy(last), joint_names, now_from_start) # Verify goal constraint result = check_goal_state() if result is True: rospy.loginfo("%s: Joint Trajectory Action Succeeded for %s arm" % (self._action_name, self._name)) self._result.error_code = self._result.SUCCESSFUL self._server.set_succeeded(self._result) elif result is False: rospy.logerr("%s: Exceeded Max Goal Velocity Threshold for %s arm" % (self._action_name, self._name)) self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED self._server.set_aborted(self._result) else: rospy.logerr("%s: Exceeded Goal Threshold Error %s for %s arm" % (self._action_name, result, self._name)) self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED self._server.set_aborted(self._result) self._command_stop(goal.trajectory.joint_names, end_angles, start_time, dimensions_dict)
def move(self): client = actionlib.SimpleActionClient('pololu_trajectory_action_server', pololu_trajectoryAction) client.wait_for_server() self._image_subs = rospy.Subscriber("detectedObjects", Objects, self.im_callback); self._sb = rospy.Subscriber("/pololu/motor_states", MotorStateList, self.IR_callback); while(not rospy.is_shutdown()): (objectDetected, area, imTime) = ImageObjects.getDetectedObject(self); (frontDistance, sideDistance, frontSlope, sideSlope, iRTime) = IR_Subscriber.getIRDistances(self); if self._lastIRTime == iRTime: #or self._lastImageTime == imTime: continue; else: self._lastImageTime = imTime; self._lastIRTime = iRTime; deltaDistance = frontDistance, sideDistance print "frontDistance: ", frontDistance," sideDistance: ",sideDistance," _lastIRTime: ",self._lastIRTime #, objectDetected," ", area, " _lastImageTime:", self._lastImageTime, currTime = rospy.get_time() goal = pololu_trajectoryGoal() traj=goal.joint_trajectory traj.header.stamp = rospy.Time.now() traj.joint_names.append(names[0]) pts=JointTrajectoryPoint() pts.time_from_start=rospy.Duration(0) if objectDetected != "None" and not self._rollingBallTrajectoryActive: if objectDetected == "Rolling_ball": print "Detected Rolling Ball." self.initiateTrajectoryToAvoidObstacle(self._lastIRTime); else: print "Stop sign." self.stop(pts); elif self._rollingBallTrajectoryActive: if (self._lastIRTime.secs - self._rollingBallTrajectoryStartTime) > 3: self._rollingBallTrajectoryActive = False; setOriginalThresholdValues(); probablyTurning = False; if frontDistance >= INFINITY_DIST: # No wall in front if self._turningRight or self._turningLeft: #Now there is no wall in front. So, set "turningRight" to False. self._turningRight = False; self._turningLeft = False; if (currTime - self._lastTurningTime) <= 5.0 and sideDistance >= INFINITY_DIST: sideDistance = Min_side_threshold_dist-1.0; if sideDistance > Max_side_threshold_dist: self.moveMotorRight(sideDistance,pts) elif sideDistance < Min_side_threshold_dist: self.moveMotorLeft(sideDistance, pts) else: self.moveMotorCenter(pts, sideSlope, currTime) else: if frontDistance < Min_front_threshold_dist: #and (currTime - self._lastTurningTime) > 3.0: if self._turningRight: self.moveMotorRight(Max_right_limit,pts); else: self.moveMotorLeft(Max_left_limit,pts); self._turningLeft = True; self._lastTurningTime = currTime; #Save turning time elif sideDistance > Max_side_threshold_dist: if sideDistance >= INFINITY_DIST: #and (currTime - self._lastTurningTime) > 3.0: #If no wall is detected by right side sensor, then take right turn self._turningRight = True; self._lastTurningTime = currTime; #Save turning time self.moveMotorRight(sideDistance, pts) elif sideDistance < Min_side_threshold_dist: self.moveMotorLeft(sideDistance, pts) else: self.moveMotorCenter(pts, sideSlope, currTime) probablyTurning = True; pts.velocities.append(1.0) traj.joint_names.append(names[1]) #DC Motor speed = 0.0; if probablyTurning: speed = 0.43 - self._speedOffset; self._speedOffset += 0.003 else: speed = MAX_SPEED + ((MIN_SPEED - MAX_SPEED)*self._speedOffset/100.0) self._speedOffset = 0.0 #speed = MIN_SPEED if speed > MIN_SPEED else speed; speed = MAX_SPEED if speed < MAX_SPEED else speed; print "DC Motor Speed: ", speed pts.positions.append(speed); pts.velocities.append(1.0) print "=======================================================================" traj.points.append(pts) client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(0.001))
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()