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 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 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 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 control_loop(self): if self.commands is None: raise Exception('Command list is empty. Was the control plan loaded?') # loop through the command list for cmd in self.commands: # wait for proxy to be in active state self.wait_for_state(self._proxy_state_running) # process commands cmd_spec_str = None spec = None t = cmd[ProxyCommand.key_command_type] if not (t == "noop"): cmd_spec_str = cmd[ProxyCommand.key_command_spec] if not isinstance(cmd_spec_str, basestring): spec = float(cmd_spec_str) else: spec = self.positions[cmd_spec_str] rospy.loginfo("Command type: " + t + ", spec: " + str(cmd_spec_str) + ", value: " + str(spec)) # check for any wait depends self.wait_for_depend(cmd) # execute command # could do this with a dictionary-based function lookup, but who cares if t == 'noop': rospy.loginfo("Command type: noop") self.wait_for_depend(cmd) elif t == 'sleep': rospy.loginfo("sleep command") v = float(spec) rospy.sleep(v) elif t == 'move_gripper': rospy.loginfo("gripper command") self.move_gripper(spec) elif t == 'move_arm': rospy.loginfo("move_arm command") rospy.logdebug(spec) goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = self.arm_joint_names jtp = JointTrajectoryPoint() jtp.time_from_start = rospy.Duration(0.5) # fudge factor for gazebo controller jtp.positions = spec jtp.velocities = [0]*len(spec) goal.trajectory.points.append(jtp) self._arm_goal = copy.deepcopy(goal) self.move_arm() elif t == 'plan_exec_arm': rospy.loginfo("plan and execute command not implemented") raise NotImplementedError() else: raise Exception("Invalid command type: " + str(cmd.type)) # check for any set dependencies action self.set_depend(cmd) # check for any clear dependencies action self.clear_depend(cmd)
def __init__(self, arm = 'l', record_data = False): self.arm = arm if arm == 'r': print "using right arm on Darci" self.RIGHT_ARM = 0 # i don't think this was used anywhere else: self.LEFT_ARM = 1 # print "Left and both arms not implemented in this client yet ... \n" # print "will require writing different function calls since joint data for left and right arm come in together from the meka server" # sys.exit() self.kinematics = dak.DarciArmKinematics(arm) self.joint_pub = rospy.Publisher('/'+self.arm+'_arm_controller/command', JointTrajectory) self.joint_names = None self.lock = RLock() self.joint_angles = None self.joint_velocities = None self.J_h = None self.time = None self.desired_joint_angles = None self.stiffness_percent = 0.75 self.ee_force = None self.ee_torque = None self.skins = None #will need code for all of these skin interfaces self.Jc_l = [] self.n_l = [] self.values_l = [] #values from m3gtt repository in robot_config folder # These could be read in from a yaml file like this (import yaml; stream = open("FILE_NAME_HERE", 'r'); data = yaml.load(stream)) # However, not clear if absolute path to yaml file (possibly on another computer) is better then just defining it here # The downside is obviously if someone tunes gains differently on the robot. self.joint_stiffness = (np.array([1, 1, 1, 1, 0.06, 0.08, 0.08])*180/np.pi*self.stiffness_percent).tolist() self.joint_damping = (np.array([0.06, 0.1, 0.015, 0.015, 0.0015, 0.002, 0.002])*180/np.pi*self.stiffness_percent).tolist() self.record_data = record_data if self.record_data: from collections import deque self.q_record = deque() self.qd_record = deque() self.times = deque() self.state_sub = rospy.Subscriber('/'+self.arm+'_arm_controller/state', JointTrajectoryControllerState, self.robotStateCallback) rospy.sleep(1.0) while self.joint_angles is None: rospy.sleep(0.05) self.desired_joint_angles = copy.copy(self.joint_angles) self.joint_cmd = JointTrajectory() self.joint_cmd.header.stamp = rospy.Time.now() self.joint_cmd.header.frame_id = '/torso_lift_link' self.joint_cmd.joint_names = self.joint_names jtp = JointTrajectoryPoint() jtp.positions = self.desired_joint_angles jtp.velocities = [1.]*len(self.joint_names) self.joint_cmd.points = [jtp] self.joint_pub.publish(self.joint_cmd)
def 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 test_5(self): # Test 5: move each joint in robotic arm to a specific value at the same time (mixed speeds) rospy.loginfo("Begin test 5") # Create a joint state publisher self.arm_pub = rospy.Publisher('arm_controller/command', JointTrajectory) rospy.sleep(1) # How fast will we update the robot's movement? in Hz rate = 10 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Define a joint trajectory message for grasp position rospy.loginfo("Position arm in stowed position") msg = JointTrajectory() msg.header.seq = 1 msg.header.stamp = rospy.Time.now() msg.header.frame_id = '0' msg.joint_names.append('joint_1') msg.joint_names.append('joint_2') msg.joint_names.append('joint_3') msg.joint_names.append('joint_4') msg.joint_names.append('joint_5') # Define joint trajectory points point = JointTrajectoryPoint() point.positions = [0.0, -1.3, 1.7, 1.2, 0.1] point.velocities = [90.0, 50.0, 20.0, 10.0, 10.0] point.time_from_start = rospy.Duration(3.0) msg.points.append(point) # publish joint stage message self.arm_pub.publish(msg) rospy.loginfo("End test 5")
def _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 add_point(self, positions, velocities, accelerations, time): point = JointTrajectoryPoint() point.positions = copy(positions) point.velocities = copy(velocities) point.accelerations = copy(accelerations) point.time_from_start = rospy.Duration(time) self._goal.trajectory.points.append(point)
def 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 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 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 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 runTrajectory(req): global Traj_server; print "---------------------------------" print req.task print " " print req.bin_num print " " print req.using_torso print "---------------------------------" if req.using_torso == "y": file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num); else: file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num); if req.task == "Forward": file_name = file_root+"/forward"; elif req.task == "Drop": file_name = file_root+"/drop"; else : return taskResponse(False); f = open(file_name,"r"); plan_file = RobotTrajectory(); buf = f.read(); plan_file.deserialize(buf); plan = copy.deepcopy(plan_file); arm_right_group = moveit_commander.MoveGroupCommander("arm_right"); arm_left_group = moveit_commander.MoveGroupCommander("arm_left"); arm_left_group.set_start_state_to_current_state(); StartPnt = JointTrajectoryPoint(); StartPnt.positions = arm_left_group.get_current_joint_values(); StartPnt.velocities = [0]*len(StartPnt.positions); StartPnt. accelerations = [0]*len(StartPnt.positions); #print StartPnt; plan.joint_trajectory.points[0] = StartPnt; #print plan; display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory) display_trajectory = moveit_msgs.msg.DisplayTrajectory(); robot = moveit_commander.RobotCommander(); display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while", file_name, " is visualized (again)..." arm_left_group.execute(plan); print "Trajectory ", file_name, " finished!" f.close(); return taskResponse(True);
def stage_3(self): # Move arm to grasping position rospy.loginfo("Begin stage 3") # Create a joint state publisher self.arm_pub = rospy.Publisher('arm_controller/command', JointTrajectory) rospy.sleep(1) # How fast will we update the robot's movement? in Hz rate = 10 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Define a joint trajectory message for grasp position rospy.loginfo("Position arm in grasp position") msg = JointTrajectory() msg.header.seq = 1 msg.header.stamp = rospy.Time.now() msg.header.frame_id = '0' msg.joint_names.append('joint_1') msg.joint_names.append('joint_2') msg.joint_names.append('joint_3') msg.joint_names.append('joint_4') msg.joint_names.append('joint_5') # Define joint trajectory points point = JointTrajectoryPoint() point.positions = [0.0, 0.0, 0.0, 1.6, 0.1] point.velocities = [20.0, 20.0, 20.0, 10.0, 10.0] point.time_from_start = rospy.Duration(3.0) msg.points.append(point) # publish joint stage message self.arm_pub.publish(msg) rospy.sleep(3)
def test_trajectory(self): # our goal variable goal = FollowJointTrajectoryGoal() # First, the joint names, which apply to all waypoints goal.trajectory.joint_names = ('shoulder_pitch_joint', 'shoulder_pan_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_pitch_joint', 'wrist_roll_joint') # We will have two waypoints in this goal trajectory # First trajectory point # To be reached 1 second after starting along the trajectory point = JointTrajectoryPoint() point.positions = [0.0] * 7 point.velocities = [1.0] * 7 point.time_from_start = rospy.Duration(3.0) goal.trajectory.points.append(point) #we are done; return the goal return goal
def talker(): if 1 == 0: pub = rospy.Publisher("/ihmc_ros/atlas/control/arm_joint_trajectory2", JointTrajectory, queue_size=10) jn = ["l_arm_shz", "l_arm_shx", "l_arm_ely", "l_arm_elx", "l_arm_wry", "l_arm_wrx", "l_arm_wry2"] jn_r = ["r_arm_shz", "r_arm_shx", "r_arm_ely", "r_arm_elx", "r_arm_wry", "r_arm_wrx", "r_arm_wry2"] else: pub = rospy.Publisher("/ihmc_ros/valkyrie/control/arm_joint_trajectory2", JointTrajectory, queue_size=10) jn = [ "LeftShoulderPitch", "LeftShoulderRoll", "LeftShoulderYaw", "LeftElbowPitch", "LeftForearmYaw", "LeftWristRoll", "LeftWristPitch", ] jn_r = [ "RightShoulderPitch", "RightShoulderRoll", "RightShoulderYaw", "RightElbowPitch", "RightForearmYaw", "RightWristRoll", "RightWristPitch", ] # this doesnt work: # jn = ["l_arm_shz","l_arm_shx","l_arm_ely","l_arm_elx","l_arm_wry","l_arm_wrx","l_arm_wry2", "r_arm_shz","l_arm_shx","r_arm_ely","r_arm_elx","r_arm_wry","r_arm_wrx","r_arm_wry2"] # value = 0 rospy.init_node("send_arm_test", anonymous=True) rate = rospy.Rate(1) # 10hz while not rospy.is_shutdown(): msg = JointTrajectory() value = 0.1 value_r = 0.1 msg.joint_names = jn pt = JointTrajectoryPoint() pt.positions = [value] * len(jn) pt.velocities = [0] * len(jn) pt.accelerations = [0] * len(jn) pt.effort = [0] * len(jn) pt.time_from_start = rospy.Duration.from_sec(3) msg.points.append(pt) print msg.joint_names, rospy.get_time() pub.publish(msg) # TODO: add a sleep here between left and right msg.joint_names = jn_r msg.points[0].positions = [value_r] * len(jn) print msg.joint_names, rospy.get_time() pub.publish(msg) rate.sleep()
def up_msg(self): jtm = JointTrajectory() jtm.joint_names = self.joint_names jtp = JointTrajectoryPoint() jtp.positions = [1.]*len(self.joint_names) jtp.velocities = [0.]*len(self.joint_names) jtp.time_from_start = rospy.Duration(5.0) jtm.points = [jtp] return jtm
def random_msg(self): jtm = JointTrajectory() jtm.joint_names = self.joint_names jtp = JointTrajectoryPoint() jtp.positions = 2*np.random.random(len(self.joint_names)) - 1 jtp.velocities = [0.]*len(self.joint_names) jtp.time_from_start = rospy.Duration(5.) jtm.points = [jtp] return jtm
def test_controller(self): from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint topic = "/pr2/torso_controller/command" rospy.init_node("test_pr2_torso_controller", log_level=rospy.DEBUG, disable_signals=True) rospy.loginfo("Preparing to publish on %s" % topic) ctl = rospy.Publisher(topic, JointTrajectory) self.assertEquals(getjoint("torso_lift_joint"), 0.0) duration = 0.1 traj = JointTrajectory() traj.joint_name = "torso_lift_joint" initialpoint = JointTrajectoryPoint() initialpoint.positions = 0.0 initialpoint.velocities = 0.0 initialpoint.time_from_start = rospy.Duration(0.0) finalpoint = JointTrajectoryPoint() finalpoint.positions = 0.195 finalpoint.velocities = 0.0 finalpoint.time_from_start = rospy.Duration(duration) # First, move up traj.points = [initialpoint, finalpoint] ctl.publish(traj) time.sleep(duration + 0.1) self.assertEquals(getjoint("torso_lift_joint"), 0.195) # Go back to initial position finalpoint.time_from_start = rospy.Duration(0.0) initialpoint.time_from_start = rospy.Duration(duration) traj.points = [finalpoint, initialpoint] ctl.publish(traj) time.sleep(duration + 0.1) self.assertEquals(getjoint("torso_lift_joint"), 0.0)
def set_position(self,goal): traj = JointTrajectory() traj_p = JointTrajectoryPoint() traj.joint_names = ["torso_lift_joint"] traj_p.positions = [goal] traj_p.velocities = [0.] traj_p.accelerations = [0.] traj_p.time_from_start = rospy.Duration(2.) traj.points.append(traj_p) self.pub.publish(traj)
def make_joint_trajectory(names, points): jt = JointTrajectory() jt.joint_names = names pt = JointTrajectoryPoint() pt.positions = points pt.effort = [0]*len(points) pt.velocities = [0]*len(points) pt.accelerations = [0]*len(points) jt.points = [pt] return jt
def np(pts, pos, time): pt = JointTrajectoryPoint() pt.positions = [pos] if len(pts)==0: st = rospy.Duration(0) else: st = pts[-1].time_from_start pt.velocities = [0] pt.time_from_start = st + rospy.Duration(time) pts.append(pt)
def create_JTG(self, arm, pos_arr, dur_arr, stamp=None, vel_arr=None, acc_arr=None): # Hack vel_arr = [[0.]*7]*len(pos_arr) acc_arr = [[0.]*7]*len(pos_arr) ## # Compute joint velocities and acclereations. def get_vel_acc(q_arr, d_arr): vel_arr = [[0.]*7] acc_arr = [[0.]*7] for i in range(1, len(q_arr)): vel, acc = [], [] for j in range(7): vel += [(q_arr[i][j] - q_arr[i-1][j]) / d_arr[i]] acc += [(vel[j] - vel_arr[i-1][j]) / d_arr[i]] vel_arr += [vel] acc_arr += [acc] print vel, acc return vel_arr, acc_arr if arm != 1: arm = 0 jtg = JointTrajectoryGoal() if stamp is None: stamp = rospy.Time.now() else: jtg.trajectory.header.stamp = stamp if len(pos_arr) > 1 and (vel_arr is None or acc_arr is None): v_arr, a_arr = get_vel_acc(pos_arr, dur_arr) if vel_arr is None: vel_arr = v_arr if acc_arr is None: acc_arr = a_arr jtg.trajectory.joint_names = self.joint_names_list[arm] for i in range(len(pos_arr)): if pos_arr[i] is None or type(pos_arr[i]) is types.NoneType: continue jtp = JointTrajectoryPoint() jtp.positions = pos_arr[i] if vel_arr is None: vel = [0.] * 7 else: vel = vel_arr[i] jtp.velocities = vel if acc_arr is None: acc = [0.] * 7 else: acc = acc_arr[i] jtp.accelerations = acc jtp.time_from_start = rospy.Duration(dur_arr[i]) jtg.trajectory.points.append(jtp) return jtg
def stage_8(self): # move arms to stowed position rospy.loginfo("Begin stage 8") # Stow robotic arm # Create a joint state publisher for each turtlebot self.arm_1_pub = rospy.Publisher('turtlebot_1/arm_controller/command', JointTrajectory) self.arm_2_pub = rospy.Publisher('turtlebot_2/arm_controller/command', JointTrajectory) rospy.sleep(1) # How fast will we update the robot's movement? in Hz rate = 10 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Define a joint trajectory message for grasp position rospy.loginfo("Position arm in stowed position") rospy.sleep(3) 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.5, 0.7, 0.0, -1.8] point.velocities = [2.0, 2.0, 20.0, 10.0, 1.0] point.time_from_start = rospy.Duration(2.0) msg.points.append(point) # Define joint trajectory points point = JointTrajectoryPoint() point.positions = [0.0, -1.0, 1.9, 1.2, 0.1] point.velocities = [50.0, 50.0, 50.0, 50.0, 10.0] point.time_from_start = rospy.Duration(3.0) msg.points.append(point) # publish joint stage message self.arm_1_pub.publish(msg) self.arm_2_pub.publish(msg) rospy.sleep(3)
def move_gripper(self, opening_mm): # Creates a goal to send to the action server. goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = self.gripper_joint_names jtp = JointTrajectoryPoint() jtp.positions = [opening_mm/2000.0, opening_mm/2000.0] jtp.velocities = [0, 0] jtp.time_from_start = rospy.Duration(1.0) goal.trajectory.points.append(jtp) self._ac_gripper.send_goal(goal) self._ac_gripper.wait_for_result() return self._ac_gripper.get_result()
def init_left_arm_cb(userdata, goal): point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(2) point.positions = [ -0.5, 0.1, -0.1, 0.6109, 0.0, 0.0, 0.0 ] point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] joint_goal = JointTrajectoryGoal() joint_goal.trajectory.joint_names = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint'] joint_goal.trajectory.header.stamp = rospy.get_rostime() joint_goal.trajectory.points.append(point) return joint_goal
def tuck_right_arm_torso_cb(userdata,goal): point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(2) point.positions = [ 0.0, 0.0, 0.3, 0.1, -1.0, 0.8, 0.0, 0.0, 0.0 ] point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] joint_goal = JointTrajectoryGoal() joint_goal.trajectory.joint_names = ['torso_1_joint', 'torso_2_joint', 'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint'] joint_goal.trajectory.header.stamp = rospy.get_rostime() joint_goal.trajectory.points.append(point) return joint_goal
def robotTrajectoryFromPlan(self, plan, joint_names): """Given a dmp plan (GetDMPPlanResponse) create a RobotTrajectory to be able to visualize what it consists and also to be able to send it to execution""" rt = RobotTrajectory() rt.joint_trajectory.joint_names = joint_names for point, time in zip(plan.plan.points, plan.plan.times): jtp = JointTrajectoryPoint() jtp.positions = point.positions jtp.velocities = point.velocities jtp.time_from_start = rospy.Duration( time ) #rospy.logwarn("jtp is: " + str(jtp)) rt.joint_trajectory.points.append(jtp) return rt
def point(): rospy.init_node('point_control') pub = rospy.Publisher('/arm_twist', JointTrajectoryPoint, 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", UInt16, vel_1_cb) rospy.Subscriber("/vel_2", UInt16, vel_2_cb) grados_joint_azim = rospy.get_param("/grados_joint_azim") rospy.loginfo("%s is %s", rospy.resolve_name('grados_joint_azim'), grados_joint_azim) grados_joint_elev = rospy.get_param("/grados_joint_elev") 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") rospy.loginfo("%s is %s", rospy.resolve_name("/offset_joint_azim"), offset_joint_azim) offset_joint_elev = rospy.get_param("/offset_joint_elev") 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 while not rospy.is_shutdown(): if (time.time() > next_point_time) and run_var == True: dt = time.time() - start_time 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) 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 main(): step_ts = 0.004 rospy.init_node("test_moveit_commander_custom_trajectory", anonymous=True) rospy.Subscriber("controller_state", ControllerState, callback_function) robot = urdf.robot_from_parameter_server() controller_commander=controller_commander_pkg.ControllerCommander() controller_commander.set_controller_mode(controller_commander.MODE_AUTO_TRAJECTORY, 0.4, [], []) time.sleep(0.5) tran = np.array([2.197026484647054, 1.2179574262842452, 0.12376598588449844]) rot = np.array([[-0.99804142, 0.00642963, 0.06222524], [ 0.00583933, 0.99993626, -0.00966372], [-0.06228341, -0.00928144, -0.99801535]]) pose_target2 = rox.Transform(rot, tran) pose_target2.p[2] += 0.20 print 'Target:',pose_target2 print "============ Move Close to Panel" controller_commander.compute_cartesian_path_and_move(pose_target2, avoid_collisions=False) controller_commander.set_controller_mode(controller_commander.MODE_AUTO_TRAJECTORY, 1.0, [], []) time.sleep(1.0) Kc = 0.004 time_save = [] FTdata_save = [] Tran_z = np.array([[0,0,-1],[0,-1,0],[1,0,0]]) Vec_wrench = 100*np.array([0.019296738361905,0.056232033265447,0.088644197659430, 0.620524934626544,-0.517896661195076,0.279323567303444,-0.059640563813256, 0.631460085138371,-0.151143175570223,-6.018321330845553]).transpose() listener=PayloadTransformListener() rapid_node = rapid_node_pkg.RAPIDCommander() #controller_commander=controller_commander_pkg.arm_composites_manufacturing_controller_commander() time.sleep(1.0) FTdata_0 = FTdata T = listener.lookupTransform("base", "link_6", rospy.Time(0)) rg = 9.8*np.matmul(np.matmul(T.R,Tran_z).transpose(),np.array([0,0,1]).transpose()) A1 = np.hstack([rox.hat(rg).transpose(),np.zeros([3,1]),np.eye(3),np.zeros([3,3])]) A2 = np.hstack([np.zeros([3,3]),rg.reshape([3,1]),np.zeros([3,3]),np.eye(3)]) A = np.vstack([A1,A2]) FTdata_0est = np.matmul(A,Vec_wrench) #print 'Test4:',controller_commander.ControllerState for i in range(400): tic = time.time() plan=RobotTrajectory() plan.joint_trajectory.joint_names=['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] current_joint_angles = controller_commander.get_current_joint_values() plan.joint_trajectory.header.frame_id='/world' p1=JointTrajectoryPoint() p1.positions = current_joint_angles p1.velocities = np.zeros((6,)) p1.accelerations = np.zeros((6,)) p1.time_from_start = rospy.Duration(0) T = listener.lookupTransform("base", "link_6", rospy.Time(0)) rg = 9.8*np.matmul(np.matmul(T.R,Tran_z).transpose(),np.array([0,0,1]).transpose()) A1 = np.hstack([rox.hat(rg).transpose(),np.zeros([3,1]),np.eye(3),np.zeros([3,3])]) A2 = np.hstack([np.zeros([3,3]),rg.reshape([3,1]),np.zeros([3,3]),np.eye(3)]) A = np.vstack([A1,A2]) FTdata_est = np.matmul(A,Vec_wrench) #print 'Test0:',FTdata,FTdata_0,FTdata_est,FTdata_0est FTread = FTdata-FTdata_0-FTdata_est+FTdata_0est print 'FTread:',FTread print 'FT:',FTdata print 'Z',FTread[-1] if FTread[-1]>-200: F_d = -350 else: F_d = -400 J = rox.robotjacobian(robot, current_joint_angles) Vz = Kc*(F_d - FTread[-1]) joints_vel = np.linalg.pinv(J).dot(np.array([0,0,0,0,0,Vz]+dx)) print 'Joint_command:',joints_vel.dot(step_ts) p2=JointTrajectoryPoint() p2.positions = np.array(p1.positions) + joints_vel.dot(step_ts)#np.array([0,np.deg2rad(-2),0,0,0,0]) p2.velocities = np.zeros((6,)) p2.accelerations = np.zeros((6,)) p2.time_from_start = rospy.Duration(step_ts) plan.joint_trajectory.points.append(p1) plan.joint_trajectory.points.append(p2) controller_commander.execute(plan) print 'Time:', time.time()-tic time_save.append(time.time()) FTdata_save.append(FTread) filename = "FTdata.txt" f_handle = file(filename, 'a') np.savetxt(f_handle, FTdata_save) f_handle.close() filename = "Time.txt" f_handle = file(filename, 'a') np.savetxt(f_handle, time_save) f_handle.close()
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 6 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() crustcrawler_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()): #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()): 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)
rospy.wait_for_service('/ve026a/set_motor') try: res = rospy.ServiceProxy('/ve026a/set_motor', SetBool)(True) print(res.message) except rospy.ServiceException, e: print("Service call failed, %s" % e) msg = JointTrajectory() msg.header.stamp = rospy.Time.now() msg.joint_names = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ] p1 = JointTrajectoryPoint() p1.positions = [0, 0, 0, 0, 0, 0] p1.velocities = [0, 0, 0, 0, 0, 0] p1.time_from_start = rospy.Duration.from_sec(3.0) p2 = JointTrajectoryPoint() p2.positions = [0.5, 0.5, 0, 0, 0, 0] p2.velocities = [0, 0, 0, 0, 0, 0] p2.time_from_start = rospy.Duration.from_sec(5.0) p3 = JointTrajectoryPoint() p3.positions = [0, 0, 0, 0, 0, 0] p3.velocities = [0, 0, 0, 0, 0, 0] p3.time_from_start = rospy.Duration.from_sec(7.0) msg.points = [p1, p2, p3] r = rospy.Rate(10)
def move_arm(self, points_list): # # Unpause the physics # rospy.wait_for_service('/gazebo/unpause_physics') # unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) # unpause_gazebo() self.client.wait_for_server() goal = FollowJointTrajectoryGoal() # We need to fill the goal message with its components # # check msg structure with: rosmsg info FollowJointTrajectoryGoal # It is composed of 4 sub-messages: # "trajectory" of type trajectory_msgs/JointTrajectory # "path_tolerance" of type control_msgs/JointTolerance # "goal_tolerance" of type control_msgs/JointTolerance # "goal_time_tolerance" of type duration trajectory_msg = JointTrajectory() # check msg structure with: rosmsg info JointTrajectory # It is composed of 3 sub-messages: # "header" of type std_msgs/Header # "joint_names" of type string # "points" of type trajectory_msgs/JointTrajectoryPoint trajectory_msg.joint_names = [ "j2n6s300_joint_1", "j2n6s300_joint_2", "j2n6s300_joint_3", "j2n6s300_joint_4", "j2n6s300_joint_5", "j2n6s300_joint_6" ] points_msg = JointTrajectoryPoint() # check msg structure with: rosmsg info JointTrajectoryPoint # It is composed of 5 sub-messages: # "positions" of type float64 # "velocities" of type float64 # "accelerations" of type float64 # "efforts" of type float64 # "time_from_start" of type duration points_msg.positions = points_list points_msg.velocities = [0, 0, 0, 0, 0, 0] points_msg.accelerations = [0, 0, 0, 0, 0, 0] points_msg.effort = [0, 0, 0, 0, 0, 0] points_msg.time_from_start = rospy.Duration(0.01) # fill in points message of the trajectory message trajectory_msg.points = [points_msg] # fill in trajectory message of the goal goal.trajectory = trajectory_msg # self.client.send_goal_and_wait(goal) self.client.send_goal(goal) self.client.wait_for_result() # wait for the robot to finish moving # rospy.sleep(2) # wait for 2s while not self.robot_has_stopped(): pass
def interpolated_ik_motion_planner_callback(self, req): #names and angles for the joints in their desired order joint_names = req.motion_plan_request.start_state.joint_state.name start_angles = req.motion_plan_request.start_state.joint_state.position #sanity-checking: joint_names and start_angles should be the same length, if any start_angles are specified if start_angles and len(joint_names) != len(start_angles): rospy.logerr("start_state.joint_state.name needs to be the same length as start_state.joint_state.position! Quitting") return 0 #reorder the start angles to the order needed by IK reordered_start_angles = [] #get the current joint states for the robot #joint_states_msg = rospy.wait_for_message('joint_states', JointState, 10.0) #if not joint_states_msg: # rospy.logerr("unable to get joint_states message") # return 0 #get the desired start angles for each IK arm joint in turn from start_state.joint_state.position #(use the current angle if not specified) for joint_name in self.ik_utils.joint_names: #desired start angle specified if joint_name in joint_names and start_angles: index = joint_names.index(joint_name) reordered_start_angles.append(start_angles[index]) else: rospy.logerr("missing joint angle, can't deal") return 0 #desired start angle not specified, use the current angle #elif 0: #joint_name in joint_states_msg.name: # index = joint_states_msg.name.index(joint_name) # current_position = joint_states_msg.position[index] # reordered_start_angles.append(current_position) #malformed joint_states message? # else: # rospy.logerr("an expected arm joint,"+joint_name+"was not found!") # return 0 #get additional desired joint angles (such as for the gripper) to pass through to IK additional_joint_angles = [] additional_joint_names = [] for (ind, joint_name) in enumerate(joint_names): if joint_name not in self.ik_utils.joint_names: #rospy.loginfo("found %s"%joint_name) additional_joint_angles.append(start_angles[ind]) additional_joint_names.append(joint_name) IK_robot_state = None if additional_joint_angles: #rospy.loginfo("adding additional start angles for:"+str(additional_joint_names)) #rospy.loginfo("additional joint angles:"+str(additional_joint_angles)) IK_robot_state = RobotState() IK_robot_state.joint_state.name = additional_joint_names IK_robot_state.joint_state.position = additional_joint_angles #check that the desired link is in the list of possible IK links (only r/l_wrist_roll_link for now) link_name = req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids[0] if link_name != self.ik_utils.link_name: rospy.logerr("link_name not allowed: %s"%link_name) return 0 #the start pose for that link start_pose = req.motion_plan_request.start_state.multi_dof_joint_state.poses[0] #the frame that start pose is in frame_id = req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids[0] #turn it into a PoseStamped start_pose_stamped = self.add_header(PoseStamped(), frame_id) start_pose_stamped.pose = start_pose #the desired goal position goal_pos = req.motion_plan_request.goal_constraints.position_constraints[0].position #the frame that goal position is in goal_pos_frame = req.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id #convert the position to base_link frame goal_ps = self.add_header(PointStamped(), goal_pos_frame) goal_ps.point = goal_pos goal_pos_list = self.ik_utils.point_stamped_to_list(goal_ps, 'base_link') #the desired goal orientation goal_quat = req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation #the frame that goal orientation is in goal_quat_frame = req.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id #convert the quaternion to base_link frame goal_qs = self.add_header(QuaternionStamped(), goal_quat_frame) goal_qs.quaternion = goal_quat goal_quat_list = self.ik_utils.quaternion_stamped_to_list(goal_qs, 'base_link') #assemble the goal pose into a PoseStamped goal_pose_stamped = self.add_header(PoseStamped(), 'base_link') goal_pose_stamped.pose = Pose(Point(*goal_pos_list), Quaternion(*goal_quat_list)) #get the ordered collision operations, if there are any ordered_collision_operations = None #req.motion_plan_request.ordered_collision_operations #if ordered_collision_operations.collision_operations == []: # ordered_collision_operations = None #get the link paddings, if there are any link_padding = None #req.motion_plan_request.link_padding #if link_padding == []: # link_padding = None #RUN! Check the Cartesian path for consistent, non-colliding IK solutions (trajectory, error_codes) = self.ik_utils.check_cartesian_path(start_pose_stamped, \ goal_pose_stamped, reordered_start_angles, self.pos_spacing, self.rot_spacing, \ self.consistent_angle, self.collision_aware, self.collision_check_resolution, \ self.steps_before_abort, self.num_steps, ordered_collision_operations, \ self.start_from_end, IK_robot_state, link_padding) #find appropriate velocities and times for the valid part of the resulting joint path (invalid parts set to 0) #if we're searching from the end, keep the end; if we're searching from the start, keep the start start_ind = 0 stop_ind = len(error_codes) if self.start_from_end: for ind in range(len(error_codes)-1, 0, -1): if error_codes[ind]: start_ind = ind+1 break else: for ind in range(len(error_codes)): if error_codes[ind]: stop_ind = ind break (times, vels) = self.ik_utils.trajectory_times_and_vels(trajectory[start_ind:stop_ind], self.max_joint_vels, self.max_joint_accs) times = [0]*start_ind + times + [0]*(len(error_codes)-stop_ind) vels = [[0]*7]*start_ind + vels + [[0]*7]*(len(error_codes)-stop_ind) rospy.logdebug("trajectory:") for ind in range(len(trajectory)): rospy.logdebug("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) rospy.logdebug("") for ind in range(len(trajectory)): rospy.logdebug("time: " + "%5.3f "%times[ind] + "vels: " + self.pplist(vels[ind])) #the response res = GetMotionPlanResponse() #the arm joint names in the normal order, as spit out by IKQuery res.trajectory.joint_trajectory.joint_names = self.ik_utils.joint_names[:] #a list of 7-lists of joint angles, velocities, and times for a trajectory that gets you from start to goal #(all 0s if there was no IK solution for a point on the path) res.trajectory.joint_trajectory.points = [] for i in range(len(trajectory)): joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = trajectory[i] joint_trajectory_point.velocities = vels[i] joint_trajectory_point.time_from_start = rospy.Duration(times[i]) res.trajectory.joint_trajectory.points.append(joint_trajectory_point) #a list of ArmNavigationErrorCodes messages, one for each trajectory point, with values as follows: #ArmNavigationErrorCodes.SUCCESS (1): no problem #ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED (-23): no non-colliding IK solution (colliding solution provided) #ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED (-20): inconsistency in path between this point and the next point #ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED (-21): out of reach (no colliding solution) #ArmNavigationErrorCodes.PLANNING_FAILED (0): aborted before getting to this point error_code_dict = {0:ArmNavigationErrorCodes.SUCCESS, 1:ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED, \ 2:ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED, 3:ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED, \ 4:ArmNavigationErrorCodes.PLANNING_FAILED} trajectory_error_codes = [ArmNavigationErrorCodes(val=error_code_dict[error_code]) for error_code in error_codes] res.trajectory_error_codes = trajectory_error_codes res.error_code.val = ArmNavigationErrorCodes.SUCCESS # rospy.loginfo("trajectory:") # for ind in range(len(trajectory)): # rospy.loginfo("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) # rospy.loginfo("") # for ind in range(len(trajectory)): # rospy.loginfo("time: " + "%5.3f "%times[ind] + "vels: " + self.pplist(vels[ind])) return res
d = rospy.Duration while not rospy.is_shutdown(): fake_traj = FollowJointTrajectoryActionGoal() fake_traj.header = Header() # fake_traj.header.stamp fake_traj.goal = FollowJointTrajectoryGoal() fake_traj.goal.trajectory = JointTrajectory() fake_traj.goal.trajectory.joint_names = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5' ] # fake_traj.goal.trajectory.points = JointTrajectoryPoint()[] for j in xrange(20): point = JointTrajectoryPoint() point.positions = [random.random() for i in xrange(5)] point.velocities = [random.random() for i in xrange(5)] point.time_from_start = rospy.Duration.from_sec(j) fake_traj.goal.trajectory.points.append(point) # fake_traj.goal.trajectory.points.positions = [[random.random() ] # fake_traj.goal.trajectory.points.velocities = [[random.random() for i in xrange(5)] for j in xrange(20)] # fake_traj.goal.trajectory.points.time_from_start = [d.from_sec(i * 0.1) for i in xrange(20)] # fake_traj.goal.trajectory.points.append(positions) # fake_traj.goal.trajectory.points.append(velocities) # fake_traj.goal.trajectory.points.append(time_from_start) pub.publish(fake_traj) rate.sleep()
g.command = goal gms.goal=g # gp.publish(gms) msg = JointTrajectory() subMsg = JointTrajectoryPoint() # print(msg) # print(subMsg) # print(type(zeros)) # solver.printDoubleVec(q_targ) print(ths) subMsg.positions = ths subMsg.velocities = zeros subMsg.accelerations = zeros msg.joint_names = joints msg.points=[subMsg] rate=rospy.Rate(1) subMsg.time_from_start = rospy.Duration(0.5) for i in range(2): msg.header.stamp = rospy.Time.now() pub.publish(msg) gp.publish(gms) rate.sleep()
from actionlib import ActionClient from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint rospy.init_node('initial_joint_state_setter', anonymous=True) ac = ActionClient('/whole_body_controller/body/follow_joint_trajectory', FollowJointTrajectoryAction) ac.wait_for_server() goal = FollowJointTrajectoryGoal() traj = JointTrajectory() traj.header.stamp = rospy.get_rostime() traj.joint_names = [ 'torso_lift_joint', 'r_upper_arm_roll_joint', 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_forearm_roll_joint', 'r_elbow_flex_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint', 'l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint', 'head_pan_joint', 'head_tilt_joint' ] p = JointTrajectoryPoint() p.positions = [ 0.1687062500000004, 0, 0, 0, 0, -1.1400000000000001, -1.0499999999999998, 0, 0, 0, 0, 0, -1.1399999999999995, -1.0499999999999998, 0, 0, 0 ] p.velocities = [0] * len(p.positions) p.time_from_start = rospy.Duration(1) traj.points.append(p) goal.trajectory = traj ac.send_goal(goal)
rospy.init_node('send_open_manipulator_gripper_joint_angles') pub = rospy.Publisher('/gripper_controller/command', JointTrajectory, queue_size=1) controller_name = "gripper_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: ['gripper'] point.positions = [-0.1] point.velocities = [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()
rospy.logerr(msg) rospy.signal_shutdown(msg) sys.exit(1) # Creates a goal to send to the action server. goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] goal.trajectory.points = [] point0 = JointTrajectoryPoint() point0.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] point0.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # To be reached 1 second after starting along the trajectory point0.time_from_start = rospy.Duration(1.0) goal.trajectory.points.append(point0) point1 = JointTrajectoryPoint() point1.positions = [-0.3, 0.2, -0.1, -1.2, 1.5, 0.5, 0.0] point1.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0] point1.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] point1.time_from_start = rospy.Duration(2.0) goal.trajectory.points.append(point1) point2 = JointTrajectoryPoint() point2.positions = [0.0, 0.1, 0.0, 0.0, 0.0, 0.1, 0.0] point2.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] point2.time_from_start = rospy.Duration(3.0)
def trayectoria_2(): pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10) rospy.init_node('trayectoria_2', anonymous=True) trajectory = JointTrajectory() trajectory.joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "gripper_revolute_joint"] trajectory.header.stamp = rospy.Time.now() trajectory.header.frame_id = "base_footprint"; #Posicion default jtp = JointTrajectoryPoint() jtp.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jtp.velocities = [0.1, 0.1, 0.1, 0.1, 0.0, 0.0] jtp.time_from_start = rospy.Duration(3) trajectory.points.append(jtp) #Aproximacion a la pieza jtp_2 = JointTrajectoryPoint() jtp_2.positions = [0.0, 0.8560, 0.5522, -1.3622, 0.0, 0.0] jtp_2.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jtp_2.time_from_start = rospy.Duration(6) trajectory.points.append(jtp_2) #Cierra la pinza jtp_3 = JointTrajectoryPoint() jtp_3.positions = [0.0, 0.8560, 0.5522, -1.3622, 0.0, 1.5] jtp_3.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jtp_3.time_from_start = rospy.Duration(9) trajectory.points.append(jtp_3) #Sube jtp_4 = JointTrajectoryPoint() jtp_4.positions = [0.0, -0.2362, -0.5844, 0.4488, 0.0, 1.5] jtp_4.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jtp_4.time_from_start = rospy.Duration(12) trajectory.points.append(jtp_4) #Giro del brazo jtp_5 = JointTrajectoryPoint() jtp_5.positions = [1.6, -0.2362, -0.5844, 0.4488, 0.0, 1.5] jtp_5.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jtp_5.time_from_start = rospy.Duration(15) trajectory.points.append(jtp_5) #Aproximacion a destino jtp_6 = JointTrajectoryPoint() jtp_6.positions = [1.6, -0.1212, -0.8375, 0.9741, 0.0, 1.5] jtp_6.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jtp_6.time_from_start = rospy.Duration(18) trajectory.points.append(jtp_6) #Suelta la pieza jtp_7 = JointTrajectoryPoint() jtp_7.positions = [1.6, -0.1212, -0.8375, 0.9741, 0.0, 0.0] jtp_7.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jtp_7.time_from_start = rospy.Duration(21) trajectory.points.append(jtp_7) #Gira de vuelta jtp_8 = JointTrajectoryPoint() jtp_8.positions = [0.0, -0.1212, -0.8375, 0.9741, 0.0, 0.0] jtp_8.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jtp_8.time_from_start = rospy.Duration(24) trajectory.points.append(jtp_8) #Posicion default jtp_9 = JointTrajectoryPoint() jtp_9.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jtp_9.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jtp_9.time_from_start = rospy.Duration(27) trajectory.points.append(jtp_9) pub.publish(trajectory)
def main(whole_body, gripper, wrist_wrench): rospy.sleep(5) armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command', JointTrajectory, queue_size=1) #1.READ THE ANGLE (topic angle_detector) #angle_Msg = rospy.wait_for_message("angle_detector", Float64) #angle = angle_Msg.data #2.READ THE HANDLE POSITION when the door is closed - SERVICE!!! #target_pose_Msg = rospy.wait_for_message("localize_handle", PoseStamped) #recog_pos.pose.position.x=target_pose_Msg.pose.position.x #recog_pos.pose.position.y=target_pose_Msg.pose.position.y #recog_pos.pose.position.z=target_pose_Msg.pose.position.z if (angle > -1 and angle < 4): #the door is closed ##3.GRAB THE DOOR HANDLE whole_body.move_to_neutral() # whole_body.impedance_config= 'grasping' switch = ImpedanceControlSwitch() # wrist_wrench.reset() gripper.command(1.0) grab_pose = geometry.multiply_tuples( geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS, y=recog_pos.pose.position.y, z=recog_pos.pose.position.z, ej=math.pi / 2), geometry.pose(ek=math.pi / 2)) whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF) wrist_wrench.reset() # whole_body.impedance_config= 'compliance_middle' switch.activate("grasping") # gripper.command(0.01) gripper.grasp(-0.008) rospy.sleep(1.0) switch.inactivate() wrist_wrench.reset() rospy.sleep(8.0) #### test manipulation whole_body.impedance_config = 'grasping' #4.ROTATE HANDLE # print(whole_body.impedance_config) # desired_rot=-1.95 # whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot}) wrist_roll = latest_positions["wrist_roll_joint"] - 0.55 traj = JointTrajectory() # This controller requires that all joints have values traj.joint_names = [ "arm_lift_joint", "arm_flex_joint", "arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] p = JointTrajectoryPoint() current_positions = [ latest_positions[name] for name in traj.joint_names ] current_positions[0] = latest_positions["arm_lift_joint"] - 0.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] = wrist_roll 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' #5.PUSH MOTION - he moves with the door - first option - streight movement phi = 90 * (2 * math.pi / 360) l = HANDLE_TO_DOOR_HINGE_POS d = 2 * l * math.cos(phi) #open_pose = geometry.pose(x = d*math.sin(phi), y=d*math.cos(phi), ek=math.pi/2 - angle_rad ) #whole_body.move_end_effector_pose(open_pose, _ROBOT_TF) #angleeee=math.pi/2 - angle_rad omni_base.go(d * math.sin(phi), d * math.cos(phi), math.pi / 2 - angle_rad, 300.0, relative=True) whole_body.move_to_neutral() elif (angle >= 4 and angle < 60): #the door is half-open #3.ROTATE PARALLEL TO THE DOOR angle_rad = angle * (2 * math.pi / 360) #omni_base.go_rel(0.0, 0.0, angle_rad , 300.0) ##4.GRAB THE DOOR HANDLE whole_body.move_to_neutral() # whole_body.impedance_config= 'grasping' switch = ImpedanceControlSwitch() # wrist_wrench.reset() gripper.command(0.0) #change coordinates of the handle - now the door is open so the handle is moved - the data of the handle position are given for the closed door - I m supposing #that the coordinates of the handle are wrt the base_footprint tf phi = math.pi / 2 - angle_rad / 2 l = HANDLE_TO_DOOR_HINGE_POS d1 = 2 * l * math.sin(angle_rad / 2) recog_pos.pose.position.x = recog_pos.pose.position.x + d1 * math.sin( phi) recog_pos.pose.position.y = recog_pos.pose.position.y + d1 * math.cos( phi) grab_pose = geometry.multiply_tuples( geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS, y=recog_pos.pose.position.y - 10, z=recog_pos.pose.position.z, ej=math.pi / 2), geometry.pose(ek=math.pi / 2)) whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF) wrist_wrench.reset() rospy.sleep(1.0) switch.inactivate() wrist_wrench.reset() rospy.sleep(8.0) #### test manipulation whole_body.impedance_config = 'grasping' #5.PUSH MOTION - he moves with the door - first option - streight movement phi = math.pi / 4 + angle_rad / 2 l = HANDLE_TO_DOOR_HINGE_POS d = 2 * l * math.cos(phi) omni_base.go(d * math.sin(phi), d * math.cos(phi), math.pi / 2 - angle_rad, 300.0, relative=True)
queue_size=10) rospy.sleep(0.5) traj = JointTrajectory() traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] now = rospy.get_rostime() rospy.loginfo("Current time %i %i", now.secs, now.nsecs) traj.header.stamp = now 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) traj.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) traj.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) traj.points.append(p3)
# wait for the action server to establish connection cli.wait_for_server() head_cli.wait_for_server() traj_goal = FollowJointTrajectoryGoal() traj = JointTrajectory() traj.joint_names = [ "odom_x", "odom_y", "odom_t", "arm_lift_joint", "arm_flex_joint", "arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] p = JointTrajectoryPoint() p.positions = [0.0, 0.0, 0.0, 0.69, -2.60, 0.0, -0.54, 0.0] p.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] p.time_from_start = rospy.Time(3) traj.points = [p] traj_goal.trajectory = traj cli.send_goal(traj_goal) cli.wait_for_result() head_traj_goal = FollowJointTrajectoryGoal() head_traj = JointTrajectory() head_traj.joint_names = ["head_pan_joint", "head_tilt_joint"] h_p = JointTrajectoryPoint()
import rospy from trajectory_msgs.msg import JointTrajectoryPoint ''' Script for interacting with group_node from an interactive sessions run as "python -i group_setpoint_control.py" Modify the fields of 'msg' in the python console to change the setpoint of the actuator group ''' if __name__ == "__main__": rospy.init_node("trajectory_gen") t_pub = rospy.Publisher("/joint_target", JointTrajectoryPoint, queue_size=1) msg = JointTrajectoryPoint() msg.positions = [0, 0, 0] msg.velocities = [0, 0, 0] msg.accelerations = [0, 0, 0] timer = rospy.Timer(rospy.Duration(0.1), lambda x: t_pub.publish(msg)) print("Modify 'msg' variable to control arm...")
def __init__(self): self.rate = rospy.Rate(10) self.sentencec = "待機中" # topics # self.sub_joint = rospy.Subscriber("/hsrb/joint_states", JointState ,self.now_pose,queue_size=10) self.sub = rospy.Subscriber("/hsrb_07/joy", Joy, self.callback, queue_size=10) rospy.Subscriber("/hsrb/wrist_wrench/compensated", WrenchStamped, self.cb_wrench_compensated, queue_size=10) self.grip_act = rospy.Publisher("/hsrb/gripper_controller/grasp/goal", GripperApplyEffortActionGoal, queue_size=10) self.pub_list = rospy.Publisher('/hsrb/gripper_controller/command', JointTrajectory, queue_size=10) self.sub_obj = rospy.Subscriber("/hsr7/recog_obj/name", String, self.recog_obj, queue_size=10) # self.vision_obj = rospy.Subscriber("/ext/vision_module/object_recognition_info", ObjectInfo, self.recog_vision,queue_size=10) self.pub_sig = rospy.Publisher( '/hsrb/gripper_trajectory_controller/command', JointTrajectory, queue_size=10) rospy.Subscriber("/AudioSentence", AudioSentence, self.set_audio, queue_size=10) # service # self.hand_traj = JointTrajectory() self.hand_sig = JointTrajectory() self.hand_traj.joint_names = ["hand_motor_joint"] self.hand_sig.joint_names = [ "hand_l_proximal_joint", "hand_r_proximal_joint" ] pp = JointTrajectoryPoint() pp.positions = [ 1.2, ] pp.velocities = [0] pp.effort = [0.1] pp.time_from_start = rospy.Time(3) self.hand_traj.points = [pp] self.hand_sig.points = [pp] self.hand_flug = True self.srv_record_start = rospy.ServiceProxy("rosbag_record", RosbagRecord) self.srv_record_stop = rospy.ServiceProxy("rosbag_record_stop", RosbagStop) self.srv_marker = rospy.ServiceProxy("marker_data_saver", Empty) self.rosbag_number = 0 self.record_flag = False self.rosbag_time = rospy.Time(0) self.record_number = 0 self.number = 0 date = time.ctime() date = date.replace(" ", " ") self.date = date.split(" ") self.image_number = 0 self.image_time = 0 # TF self.buf = tf2_ros.Buffer() self.lis = tf2_ros.TransformListener(self.buf) self._wrench = WrenchStamped() self.obj_name = "" self.obj_time = rospy.Time.now().to_sec() self.vis_obj_name = "" self.vis_obj_num = 0 self.vis_obj_time = rospy.Time.now().to_sec() self.flag_record = True
def compose_trajectory(self, component_name, parameter_name): # get joint_names from parameter server param_string = self.ns_global_prefix + "/" + component_name + "/joint_names" if not rospy.has_param(param_string): rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",param_string) return (JointTrajectory(), 2) joint_names = rospy.get_param(param_string) # check joint_names parameter if not type(joint_names) is list: # check list rospy.logerr("no valid joint_names for %s: not a list, aborting...",component_name) print "joint_names are:",joint_names return (JointTrajectory(), 3) else: for i in joint_names: #print i,"type1 = ", type(i) if not type(i) is str: # check string rospy.logerr("no valid joint_names for %s: not a list of strings, aborting...",component_name) print "joint_names are:",param return (JointTrajectory(), 3) else: rospy.logdebug("accepted joint_names for component %s",component_name) ''' # get joint values from parameter server if type(parameter_name) is str: full_parameter_name = self.ns_global_prefix + "/" + component_name + "/" + parameter_name # parameter_name: joint_state if not rospy.has_param(full_parameter_name): rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",full_parameter_name) return (JointTrajectory(), 2) param = rospy.get_param(full_parameter_name) else: param = parameter_name # check trajectory parameters if not type(param) is list: # check outer list rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name) print "parameter is:",param return (JointTrajectory(), 3) ''' param = self.points traj = [] for point in param: #print point,"type1 = ", type(point) if type(point) is str: full_parameter_name = self.ns_global_prefix + "/" + component_name + "/" + point if not rospy.has_param(full_parameter_name): rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",full_parameter_name) return (JointTrajectory(), 2) point = rospy.get_param(full_parameter_name) point = point[0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories #print point elif type(point) is list: rospy.logdebug("point is a list") else: rospy.logerr("no valid parameter for %s: not a list of lists or strings, aborting...",component_name) print "parameter is:",param return (JointTrajectory(), 3) # here: point should be list of floats/ints #print point if not len(point) == len(joint_names): # check dimension rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,len(joint_names),len(point)) print "parameter is:",param return (JointTrajectory(), 3) for value in point: #print value,"type2 = ", type(value) if not ((type(value) is float) or (type(value) is int)): # check type #print type(value) rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name) print "parameter is:",param return (JointTrajectory(), 3) rospy.logdebug("accepted value %f for %s",value,component_name) traj.append(point) rospy.logdebug("accepted trajectory for %s",component_name) # get current pos timeout = 3.0 try: start_pos = rospy.wait_for_message("/" + component_name + "/joint_states", JointState, timeout = timeout).position except rospy.ROSException as e: rospy.logwarn("no joint states received from %s within timeout of %ssec. using default point time of 8sec.", component_name, str(timeout)) start_pos = [] # convert to ROS trajectory message traj_msg = JointTrajectory() # if no timestamp is set in header, this means that the trajectory starts "now" traj_msg.joint_names = joint_names point_nr = 0 traj_time = 0 param_string = self.ns_global_prefix + "/" + component_name + "/default_vel" if not rospy.has_param(param_string): default_vel = 0.1 # rad/s rospy.logwarn("parameter %s does not exist on ROS Parameter Server, using default of %f [rad/sec].",param_string,default_vel) else: default_vel = rospy.get_param(param_string) param_string = self.ns_global_prefix + "/" + component_name + "/default_acc" if not rospy.has_param(param_string): default_acc = 1.0 # rad^2/s rospy.logwarn("parameter %s does not exist on ROS Parameter Server, using default of %f [rad^2/sec].",param_string,default_acc) else: default_acc = rospy.get_param(param_string) for point in traj: point_nr = point_nr + 1 point_msg = JointTrajectoryPoint() point_msg.positions = point # set zero velocities for last trajectory point only #if point_nr == len(traj): # point_msg.velocities = [0]*len(joint_names) # set zero velocity and accelerations for all trajectory points point_msg.velocities = [0]*len(joint_names) point_msg.accelerations = [0]*len(joint_names) # use hardcoded point_time if no start_pos available if start_pos != []: print "start pose is not defined" point_time = self.calculate_point_time(component_name, start_pos, point, default_vel, default_acc) else: print "********Hello*******" point_time = 8*point_nr start_pos = point point_msg.time_from_start=rospy.Duration(point_time + traj_time) traj_time += point_time traj_msg.points.append(point_msg) return (traj_msg, 0)
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() cli = actionlib.SimpleActionClient( '/hsrb/omni_base_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction) vel_pub = rospy.Publisher('/hsrb/command_velocity', geometry_msgs.msg.Twist, queue_size=10) 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') base = robot.get('omni_base') start_theta = base.pose[2] # 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 whole_body.move_to_neutral() grasp_point_client() global recog_pos global is_found print recog_pos.pose.position print("Opening the gripper") whole_body.move_to_neutral() rospy.sleep(2.5) switch = ImpedanceControlSwitch() gripper.command(1.0) # Approach to the door print("Approach to the door") grab_pose = geometry.multiply_tuples( geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_X, y=recog_pos.pose.position.y - HANDLE_TO_HAND_POS_Y, z=recog_pos.pose.position.z, ej=math.pi / 2), geometry.pose(ek=math.pi / 2)) whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF) # Close the gripper wrist_wrench.reset() switch.activate("grasping") gripper.grasp(-0.01) 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() current_positions = [ latest_positions[name] for name in traj.joint_names ] current_positions[0] = latest_positions["arm_lift_joint"] - 0.0675 current_positions[1] = latest_positions["arm_flex_joint"] - 0.02 current_positions[2] = latest_positions["arm_roll_joint"] current_positions[3] = latest_positions["wrist_flex_joint"] current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65 p.positions = current_positions p.velocities = [0, 0, 0, 0, 0] p.time_from_start = rospy.Time(3) traj.points = [p] armPub.publish(traj) rospy.sleep(3.0) print("finishing rotating handle") ## Move by End-effector line whole_body.impedance_config = 'compliance_hard' whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45) print("push the door") ## Move base with linear & Angular motion tw = geometry_msgs.msg.Twist() tw.linear.x = 0.9 tw.angular.z = 0.45 vel_pub.publish(tw) rospy.sleep(4.0) ## Move base with linear & Angular motion second tw_cmd0 = geometry_msgs.msg.Twist() tw_cmd0.linear.x = 0.5 tw_cmd0.angular.z = 0.45 vel_pub.publish(tw_cmd0) rospy.sleep(2.0) tw_cmd1 = geometry_msgs.msg.Twist() tw_cmd1.linear.x = 0.6 tw_cmd1.linear.y = 0.2 tw_cmd1.angular.z = 0.25 vel_pub.publish(tw_cmd1) # rospy.sleep(4.0) rospy.sleep(3.0) tw_cmd2 = geometry_msgs.msg.Twist() tw_cmd2.linear.x = 0.65 tw_cmd2.linear.y = 0.5 tw_cmd2.angular.z = 0.35 vel_pub.publish(tw_cmd2) # rospy.sleep(4.0) rospy.sleep(2.0) ## Open the gripper gripper.command(1.0) ## Move back base.go_rel(-1.3, 0.0, start_theta) 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
control_pan_pos_2 = makeSimpleProfile(control_pan_pos_2, target_pan_pos_2, (POS_STEP_SIZE / 2.0)) control_tilt_pos_2 = makeSimpleProfile(control_tilt_pos_2, target_tilt_pos_2, (POS_STEP_SIZE / 2.0)) head_msg.header.stamp = rospy.Time.now() # jtp_msg.points.positions = [control_pan_pos,control_tilt_pos,control_yaw_pos] jtp_msg.positions = [ control_pan_pos, control_tilt_pos, control_pan_pos_2, control_tilt_pos_2 ] jtp_msg.velocities = [0.5, 0.5, 0.5, 0.5] jtp_msg.time_from_start = rospy.Duration.from_sec(0.00000002) head_msg.points.append(jtp_msg) pub.publish(twist) head_pub.publish(head_msg) except: print e finally: twist = Twist() twist.linear.x = 0.0 twist.linear.y = 0.0 twist.linear.z = 0.0
def __init__(self): # First set up service request_trajectory_service = rospy.ServiceProxy( "generate_toppra_trajectory", GenerateTrajectory) # This is an example for UAV and output trajectory is converted # accordingly trajectory_pub = rospy.Publisher('multi_dof_trajectory', MultiDOFJointTrajectory, queue_size=1) time.sleep(0.5) # Example of a simple square trajectory for quadcopter. All vectors # must be of same length x = [0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0] y = [0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0] z = [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] yaw = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] # Another example. Same square defined through more points. This will # overwrite the first example x = [0.0, 0.5, 1.0, 1.0, 1.0, 0.5, 0.0, 0.0, 0.0] y = [0.0, 0.0, 0.0, 0.5, 1.0, 1.0, 1.0, 0.5, 0.0] z = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] yaw = [0, 0, 0, 0, 0, 0, 0, 0, 0] # Create a service request which will be filled with waypoints request = GenerateTrajectoryRequest() # Add waypoints in request waypoint = JointTrajectoryPoint() for i in range(0, len(x)): # Positions are defined above waypoint.positions = [x[i], y[i], z[i], yaw[i]] # Also add constraints for velocity and acceleration. These # constraints are added only on the first waypoint since the # TOPP-RA reads them only from there. if i == 0: waypoint.velocities = [2, 2, 2, 1] waypoint.accelerations = [1.25, 1.25, 1.25, 1] # Append all waypoints in request request.waypoints.points.append(copy.deepcopy(waypoint)) # Set up joint names. This step is not necessary request.waypoints.joint_names = ["x", "y", "z", "yaw"] # Set up sampling frequency of output trajectory. request.sampling_frequency = 100.0 # Set up number of gridpoints. The more gridpoints there are, # trajectory interpolation will be more accurate but slower. # Defaults to 100 request.n_gridpoints = 500 # If you want to plot Maximum Velocity Curve and accelerations you can # send True in this field. This is intended to be used only when you # have to debug something since it will block the service until plot # is closed. request.plot = False # Request the trajectory response = request_trajectory_service(request) # Response will have trajectory and bool variable success. If for some # reason the trajectory was not able to be planned or the configuration # was incomplete or wrong it will return False. print("Converting trajectory to multi dof") joint_trajectory = response.trajectory multi_dof_trajectory = self.JointTrajectory2MultiDofTrajectory( joint_trajectory) trajectory_pub.publish(multi_dof_trajectory)
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, velocities, time): point = JointTrajectoryPoint() point.positions = copy(positions) point.velocities = copy(velocities) point.time_from_start = time self._goal.trajectory.points.append(point)
omega = np.pi / time d_x = np.zeros(3) d_x[0] = -omega * r * np.sin(theta) d_x[1] = 0 d_x[2] = omega * r * np.cos(theta) # Computing devirative of velocity dd_x = np.zeros(3) dd_x[0] = -(omega**2) * r * np.cos(theta) dd_x[1] = 0 dd_x[2] = -(omega**2) * r * np.sin(theta) msg = JointTrajectoryPoint() msg.positions = [x[0], x[1], x[2]] msg.velocities = [d_x[0], d_x[1], d_x[2]] msg.accelerations = [dd_x[0], dd_x[1], dd_x[2]] pub.publish(msg) rospy.sleep(Ts) # Last trajectory point means the system is stable, i.e. no velocity nor acceleration x = np.zeros(3) x[0] = x_e[0] - 2 * r x[1] = 0 x[2] = x_e[2] d_x = np.zeros(3) dd_x = np.zeros(3)
# Hand gesture parameters do not need an array and are defined as: # # [thumb, pointer, middle, ring, pinky] # # # # Arm gesture parameters only include shoulder_medial_joint and elbow_joint. # # All others in the array should be set as 0. # # =========================================================================== # pointer = [1, 0, 1, 1, 1] peace = [1, 0, 0, 1, 1] thumbs_up = [0, 1, 1, 1, 1] fist = [1, 1, 1, 1, 1] open_hand = [0, 0, 0, 0, 0] pointer_gest = JointTrajectory() pointer_point = JointTrajectoryPoint() pointer_point.positions = pointer pointer_point.velocities = [0] * 5 pointer_gest.points = [pointer_point] hand_gesture1 = JointTrajectory() hand_gesture1.points = [JointTrajectoryPoint(positions=pointer)] hand_gesture2 = JointTrajectory() hand_gesture2.points = [JointTrajectoryPoint(positions=peace)] hand_gesture3 = JointTrajectory() hand_gesture3.points = [JointTrajectoryPoint(positions=thumbs_up)] hand_gesture4 = JointTrajectory() hand_gesture4.points = [JointTrajectoryPoint(positions=fist)] hand_gesture5 = JointTrajectory() hand_gesture5.points = [JointTrajectoryPoint(positions=open_hand)] gesture_state = String() hand_gesture = JointTrajectory()
def createTrajectoryPoint(time, positions): point = JointTrajectoryPoint() point.time_from_start = rospy.Duration(time) point.positions = positions point.velocities = ZERO_VECTOR return point
imutime = 0 #for record initialTheta = 0 #laser_scan initial theta radient initialThetalist = [] statusJP = JointTrajectoryPoint() statusJP_time = 0 pose_time = 0 ready = 0 enginevalue = 1500 steeringvalue = 1500 started = True MAX_engine = 1620 #initialize goal goal.positions = [1, 1, 0] goal.velocities = [0, 0, 0] goal.accelerations = [0, 0, 0] goal.effort = [1] #initialize statusJP statusJP.positions = [0, 0, 0] statusJP.velocities = [0, 0, 0] statusJP.accelerations = [0, 0, 0] statusJP.effort = [1] def mpcControl(initial_state, cx, cy, cyaw, cv, ca, cj): """ :param x1: :param y1: :param yaw1:
def main(whole_body, base, gripper, wrist_wrench): cli = actionlib.SimpleActionClient( '/hsrb/omni_base_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction) # publisher for delvering command for base move vel_pub = rospy.Publisher('/hsrb/command_velocity', geometry_msgs.msg.Twist, queue_size=10) # base_pub = rospy.Publisher('/move_base/move/goal',move_base_msgs.msg.MoveBaseActionGoal,queue_size=10) armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command', JointTrajectory, queue_size=1) ## Grab the handle of door #test with service - get the handle position from handle grasp_point_client() global recog_pos global Is_found print recog_pos.pose.position # target_pose_Msg = rospy.wait_for_message("/handle_detector/grasp_point", PoseStamped) # recog_pos.pose.position.x=target_pose_Msg.pose.position.x # recog_pos.pose.position.y=target_pose_Msg.pose.position.y # recog_pos.pose.position.z=target_pose_Msg.pose.position.z whole_body.move_to_neutral() # whole_body.impedance_config= 'grasping' switch = ImpedanceControlSwitch() # wrist_wrench.reset() gripper.command(1.0) grab_pose = geometry.multiply_tuples( geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_, y=recog_pos.pose.position.y - 0.012, z=recog_pos.pose.position.z, ej=math.pi / 2), geometry.pose(ek=math.pi / 2)) whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF) wrist_wrench.reset() # whole_body.impedance_config= 'compliance_middle' switch.activate("grasping") # gripper.command(0.01) gripper.grasp(-0.01) rospy.sleep(1.0) switch.inactivate() wrist_wrench.reset() rospy.sleep(8.0) #### test manipulation whole_body.impedance_config = 'grasping' # print(whole_body.impedance_config) # desired_rot=-1.95 # whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot}) traj = JointTrajectory() # This controller requires that all joints have values traj.joint_names = [ "arm_lift_joint", "arm_flex_joint", "arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] p = JointTrajectoryPoint() current_positions = [latest_positions[name] for name in traj.joint_names] current_positions[0] = latest_positions["arm_lift_joint"] - 0.07 current_positions[1] = latest_positions["arm_flex_joint"] - 0.02 current_positions[2] = latest_positions["arm_roll_joint"] current_positions[3] = latest_positions["wrist_flex_joint"] current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65 p.positions = current_positions p.velocities = [0, 0, 0, 0, 0] p.time_from_start = rospy.Time(3) traj.points = [p] armPub.publish(traj) rospy.sleep(3.0) # goal_pose =PoseStamped() # goal_pose.pose.position.x=recog_pos.pose.position.x+0.4 # goal_pose.pose.position.y=recog_pos.pose.position.y+0.2 # goal_pose.pose.position.z=recog_pos.pose.position.z # goal_pose.pose.orientation.x=recog_pos.pose.orientation.x # goal_pose.pose.orientation.y=recog_pos.pose.orientation.y # goal_pose.pose.orientation.z=recog_pos.pose.orientation.z # goal_pose.pose.orientation.w=recog_pos.pose.orientation.w # yaw=math.pi/6 # quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw) # #type(pose) = geometry_msgs.msg.Pose # nav_goal = move_base_msgs.msg.MoveBaseActionGoal() # nav_goal.header.stamp=rospy.Time(0) # nav_goal.goal.target_pose.header.frame_id = "map" # nav_goal.goal.target_pose.pose.position.x=recog_pos.pose.position.x+0.3 # nav_goal.goal.target_pose.pose.position.y=recog_pos.pose.position.y+0.2 # nav_goal.goal.target_pose.pose.position.z=0.0 # nav_goal.goal.target_pose.pose.orientation.x=quaternion[0] # nav_goal.goal.target_pose.pose.orientation.y=quaternion[1] # nav_goal.goal.target_pose.pose.orientation.z=quaternion[2] # nav_goal.goal.target_pose.pose.orientation.w=quaternion[3] # base_pub.publish(nav_goal) # # whole_body.end_effector_frame = u'odom' # # whole_body.move_end_effector_by_line((0, 0, 1), -0.2) # # publish_arm(latest_positions["arm_lift_joint"],latest_positions["arm_flex_joint"],latest_positions["arm_roll_joint"], latest_positions["wrist_flex_joint"],wrist_roll) whole_body.impedance_config = 'compliance_hard' whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45) # # whole_body.move_end_effector_by_line((0.7071, 0.7071,0), 0.5) # # whole_body.move_end_effector_by_line((0, -0.7071, 0.7071), 0.5) # whole_body.impedance_config= None tw = geometry_msgs.msg.Twist() tw.linear.x = 0.9 tw.angular.z = 0.45 vel_pub.publish(tw) rospy.sleep(4.0) tw_cmd0 = geometry_msgs.msg.Twist() tw_cmd0.linear.x = 0.3 tw_cmd0.angular.z = 0.45 vel_pub.publish(tw_cmd0) # rospy.sleep(4.0) rospy.sleep(4.0) gripper.command(1.0) # whole_body.move_end_effector_by_line((0, 0, -1), 0.25) tw_cmd = geometry_msgs.msg.Twist() tw_cmd.linear.x = -1.2 vel_pub.publish(tw_cmd) rospy.sleep(2.0) tw_cmd2 = geometry_msgs.msg.Twist() tw_cmd2.linear.x = -1.1 tw_cmd2.angular.z = -0.4 vel_pub.publish(tw_cmd2) rospy.sleep(4.0) whole_body.move_to_neutral() tw_cmd2 = geometry_msgs.msg.Twist() tw_cmd2.linear.x = -0.6 tw_cmd2.angular.z = -0.3 vel_pub.publish(tw_cmd2)
# DEFINITIONS # # --------------------------------------------------------------------------- # # Arm gesture parameters only include shoulder_medial_joint and elbow_joint. # # All others in the array should be set empty. # # 2 is the max value for elbow_joint, while -2 is the min # # 3.2 indicates a 180 degree rotation for shoulder_medial_joint # # =========================================================================== # arm_names = ['shoulder_medial_joint', 'elbow_joint'] # Blank gesture that is set depending on the desired position in the main loop arm_gesture = JointTrajectory() arm_gesture.joint_names = arm_names arm_gesture_points = JointTrajectoryPoint() arm_gesture_points.positions = [] arm_gesture_points.velocities = [] arm_gesture_points.accelerations = [] arm_gesture_points.effort = [] arm_gesture_points.time_from_start = rospy.Duration(3) arm_gesture.points = [arm_gesture_points] position = String() # point locations # test = [-1.37, -1.54] test = [-1.6, -1.3] default = [0, 0] max_left = [0, 2] max_right = [3.2, 2] middle = [1.6, 1]
def get_trajectory_point(positions, time_from_start): point = JointTrajectoryPoint() point.positions = positions point.velocities = [.0] * len(positions) point.time_from_start = time_from_start return point