def _pub_joint_velocities(self, velocities): command_msg = JointCommand() command_msg.names = self.arm_joint_names command_msg.velocity = velocities command_msg.mode = JointCommand.VELOCITY_MODE command_msg.header.stamp = rospy.Time.now() self.joint_pub.publish(command_msg)
def _pub_joint_torques(self, torques): command_msg = JointCommand() command_msg.names = self.arm_joint_names command_msg.effort = torques command_msg.mode = JointCommand.TORQUE_MODE command_msg.header.stamp = rospy.Time.now() self.joint_pub.publish(command_msg)
def _pub_joint_positions(self, positions): command_msg = JointCommand() command_msg.names = self.arm_joint_names command_msg.position = positions command_msg.mode = JointCommand.POSITION_MODE command_msg.header.stamp = rospy.Time.now() self.joint_pub.publish(command_msg)
def _set_joint_velocities(self, actions): if self.vel_mode == "raw": velocities = dict() enum_iter = enumerate(self.cmd_names, start=0) for i, jnt in enum_iter: velocities[jnt] = actions[i] command_msg = JointCommand() command_msg.names = velocities.keys() command_msg.velocity = velocities.values() command_msg.mode = JointCommand.VELOCITY_MODE command_msg.header.stamp = rospy.Time.now() self.jnt_cm_pub.publish(command_msg) else: # Command Sawyer's joints to angles as calculated by velocity*dt positions = dict() q_jnt_angles = copy.copy(self.init_pos) obs_prev = self._get_cur_obs() enum_iter = enumerate(self.jnt_indices, start=0) for i, index in enum_iter: timestep = self.dt + np.random.normal(0, 1) val = obs_prev[i] + actions[i] * timestep val = np.clip(val, self.jnt_pos_limits[i][0], self.jnt_pos_limits[i][1]) q_jnt_angles[index] = val enum_iter = enumerate(self.all_jnts, start=0) for i, jnt_name in enum_iter: positions[jnt_name] = q_jnt_angles[i] self.limb.move_to_joint_positions(positions)
def basicPositionMoveForPositioning(self, pos, speed): pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) sub = rospy.Subscriber('/robot/joint_states', JointState, self.joints_callback) rate = rospy.Rate(180) # define the speed publisher pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio', Float64, latch=True, queue_size=10) command = JointCommand() command.names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] command.position = [ pos['right_j0'], pos['right_j1'], pos['right_j2'], pos['right_j3'], pos['right_j4'], pos['right_j5'], pos['right_j6'] ] command.mode = 1 # customize the value to change speed pub_speed_ratio.publish(speed) # terminate the control once the arm moved to the desired joint space within the threshold pub.publish(command) rate.sleep() rospy.loginfo( ">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def movetozero(self): rospy.init_node('arm_low_level_control', anonymous=True) pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio', Float64, latch=True, queue_size=10) command = JointCommand() command.names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] command.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] command.mode = 1 pub_speed_ratio.publish(0.2) start_time = rospy.get_time() end_time = rospy.get_time() rospy.loginfo( ">>>>>>>>>> moving the arm to zero joint position >>>>>>>>>>>") while end_time - start_time < 8: pub.publish(command) end_time = rospy.get_time()
def main(): try: rospy.init_node('avalos_limb_py') rate = rospy.Rate(100) pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) #limb = Limb() my_msg = JointCommand() # POSITION_MODE my_msg.mode = 1 my_msg.names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] my_msg.position = [0, 0, 0, 0, 0, 0, 0] for x in xrange(100): pub.publish(my_msg) rate.sleep() print "Move ok" except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user.')
def _send_pos_command(self, pos): self._try_enable() command = JointCommand() command.mode = JointCommand.POSITION_MODE command.names = self._limb.joint_names() command.position = pos self._cmd_publisher.publish(command)
def bring2stretched(self): _robot = JointCommand() _robot.mode = 1 _robot.names = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'] _robot.position = [0,0,-1.61,0,0,0,0] for i in range(1000): self.pub_joints.publish(_robot) time.sleep(0.01)
def talker2(): rospy.init_node('joint_state_publisher_0') pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) rate = rospy.Rate(10) # 10hz hello_str = JointCommand() hello_str.header = Header() hello_str.header.stamp = rospy.Time.now() hello_str.mode = 1 hello_str.names = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'] # hello_str.position = [-0.20288, 0.61753, -1.74399, -1.32579, 1.09791, -1.74968, 0.51062] # hello_str.position = [-1.20675, 0.467, -3.04262, -0.39422, 2.96331, -2.21461, 2.87877] positions = [[-0.20288, 0.61753, -1.74399, -1.32579, 1.09791, -1.74968, 0.51062], [-0.36308, 0.48712, -1.88532, -1.08158, 1.30146, -1.8188, 0.22098], [-0.39752, 0.44012, -1.93739, -0.98365, 1.25842, -1.75613, 0.24877], [-0.43197, 0.39311, -1.98946, -0.88573, 1.21539, -1.69347, 0.27656], [-0.46642, 0.3461, -2.04154, -0.7878, 1.17236, -1.6308, 0.30435], [-0.50087, 0.29909, -2.09361, -0.68987, 1.12933, -1.56814, 0.33214], [-0.53532, 0.25209, -2.14568, -0.59195, 1.08629, -1.50547, 0.35993], [-0.56976, 0.20508, -2.19776, -0.49402, 1.04326, -1.4428, 0.38772], [-0.60421, 0.15807, -2.24983, -0.3961, 1.00023, -1.38014, 0.41551], [-0.63936, 0.16653, -2.29415, -0.3935, 1.11641, -1.42953, 0.56089], [-0.67451, 0.17499, -2.33848, -0.39091, 1.2326, -1.47893, 0.70627], [-0.70966, 0.18346, -2.3828, -0.38832, 1.34879, -1.52832, 0.85165], [-0.74481, 0.19192, -2.42712, -0.38573, 1.46497, -1.57772, 0.99702], [-0.77996, 0.20038, -2.47145, -0.38314, 1.58116, -1.62711, 1.1424], [-0.81511, 0.20884, -2.51577, -0.38055, 1.69735, -1.67651, 1.28778], [-0.85026, 0.2173, -2.56009, -0.37796, 1.81354, -1.7259, 1.43316], [-0.88541, 0.22576, -2.60442, -0.37536, 1.92972, -1.7753, 1.57853], [-0.92056, 0.23422, -2.64874, -0.37277, 2.04591, -1.82469, 1.72391], [-0.95571, 0.24268, -2.69306, -0.37018, 2.1621, -1.87409, 1.86929], [-0.99086, 0.25114, -2.73739, -0.36759, 2.27828, -1.92348, 2.01467], [-1.02601, 0.2596, -2.78171, -0.365, 2.39447, -1.97288, 2.16004], [-1.06116, 0.26806, -2.82603, -0.36241, 2.51066, -2.02227, 2.30542], [-1.0963, 0.27652, -2.87036, -0.35981, 2.62684, -2.07167, 2.4508], [-1.13145, 0.28498, -2.91468, -0.35722, 2.74303, -2.12106, 2.59618], [-1.1666, 0.29344, -2.959, -0.35463, 2.85922, -2.17045, 2.74155], [-1.20175, 0.30191, -3.00333, -0.35204, 2.9754, -2.21985, 2.88693]] hello_str.velocity = [] hello_str.effort = [] # pub.publish(hello_str) # time.sleep(10) step = 0 while not rospy.is_shutdown(): hello_str.header.stamp = rospy.Time.now() hello_str.position = positions[step] pub.publish(hello_str) step +=1 if(step >= len(positions)): break time.sleep(2) rate.sleep()
def lowerArmBasicMove(self, cur_pos, speed): # define some parameters: frequency = 100 # customize this to change the vibration speed repetation = 20 pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) rate = rospy.Rate(frequency) pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio', Float64, latch=True, queue_size=10) # set the joint angles for the first press position command1 = JointCommand() command1.names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] command1.position = [ cur_pos['right_j0'], cur_pos['right_j1'], cur_pos['right_j2'], cur_pos['right_j3'] - 0.05, cur_pos['right_j4'] - 0.05, cur_pos['right_j5'] + 0.1, cur_pos['right_j6'] ] command1.mode = 1 # set the joint angles for the second press position command2 = JointCommand() command2.names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] command2.position = [ cur_pos['right_j0'], cur_pos['right_j1'], cur_pos['right_j2'], cur_pos['right_j3'], cur_pos['right_j4'], cur_pos['right_j5'], cur_pos['right_j6'] ] command2.mode = 1 # looping to create vibrating motion pub_speed_ratio.publish(speed) for i in range(0, repetation): self.forcePress(self.vibration_force, 0.1) #for j in range(0, 10): # pub.publish(command1) # rate.sleep() #rate.sleep() for k in range(0, 10): pub.publish(command2) rate.sleep() rate.sleep()
def _set_joint_torques(self, actions): torques = dict() enum_iter = enumerate(self.all_jnts, start=0) for i, jnt_name in enum_iter: torques[jnt_name] = 0.0 enum_iter = enumerate(self.cmd_names, start=0) for i, jnt_name in enum_iter: torques[jnt_name] = actions[i] command_msg = JointCommand() command_msg.names = torques.keys() command_msg.effort = torques.values() command_msg.mode = JointCommand.TORQUE_MODE command_msg.header.stamp = rospy.Time.now() self.jnt_cm_pub.publish(command_msg)
def basicTrajMove(self, positions, speed, traj_length, speed_rate): pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) sub = rospy.Subscriber('/robot/joint_states', JointState, self.joints_callback) rate = rospy.Rate(speed_rate) # define the speed publisher pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio', Float64, latch=True, queue_size=10) command = JointCommand() command.names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] command.mode = 1 # customize the value to change speed pub_speed_ratio.publish(speed) control_diff_record = 10.0 control_diff_temp = 0.0 threshold = 0.015 # terminate the control once the arm moved to the desired joint space within the threshold counter = 0 while control_diff_record > threshold and counter < traj_length - 1: if positions[counter] == False: continue command.position = [ positions[counter]['right_j0'], positions[counter]['right_j1'], positions[counter]['right_j2'], positions[counter]['right_j3'], positions[counter]['right_j4'], positions[counter]['right_j5'], positions[counter]['right_j6'] ] pub.publish(command) for x, y in zip(self.jointAngles, command.position): control_diff_temp = abs(x - y) + control_diff_temp control_diff_record = control_diff_temp control_diff_temp = 0.0 rate.sleep() counter = counter + 1 rospy.loginfo( ">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def basicPositionMove(self, pos, speed): pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) sub = rospy.Subscriber('/robot/joint_states', JointState, self.joints_callback) rate = rospy.Rate(180) # define the speed publisher pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio', Float64, latch=True, queue_size=10) command = JointCommand() command.names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] command.position = [ pos['right_j0'], pos['right_j1'], pos['right_j2'], pos['right_j3'], pos['right_j4'], pos['right_j5'], pos['right_j6'] ] command.mode = 1 # customize the value to change speed pub_speed_ratio.publish(speed) control_diff_record = 10.0 control_diff_temp = 0.0 threshold = 0.02 # terminate the control once the arm moved to the desired joint space within the threshold start_time = rospy.get_time() end_time = rospy.get_time() while control_diff_record > threshold: end_time = rospy.get_time() if end_time - start_time > 5: break pub.publish(command) for x, y in zip(self.jointAngles, command.position): control_diff_temp = abs(x - y) + control_diff_temp control_diff_record = control_diff_temp control_diff_temp = 0.0 rate.sleep() rospy.loginfo( ">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def basicPositionMove(self, pos, speed, initial): pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) sub = rospy.Subscriber('/robot/joint_states', JointState, self.joints_callback) # define the speed publisher pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio', Float64, latch=True, queue_size=10) command = JointCommand() command.names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] command.position = [ pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6] ] command.mode = 1 # customize the value to change speed pub_speed_ratio.publish(speed) # terminate the control once the arm moved to the desired joint space within the threshold rate = rospy.Rate(1000) control_diff_record = 10.0 control_diff_temp = 0.0 threshold = 0.02 if not initial: # terminate the control once the arm moved to the desired joint space within the threshold for i in range(0, 200): pub.publish(command) rate.sleep() else: for i in range(0, 10000): rospy.logerr("returning to the first point") pub.publish(command) rate.sleep() rospy.loginfo( ">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def basicAccelerationMove(self, pos, speed): pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio', Float64, latch=True, queue_size=10) sub = rospy.Subscriber('/robot/joint_states', JointState, self.joints_callback) command = JointCommand() command.names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] command.position = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] command.acceleration = [0.3, 0.05, 0.05, 0.05, 0.05, 0.05, 0.1] command.mode = 1 #pub_speed_ratio.publish(0.2) rate = rospy.Rate(10) control_diff_record = 10.0 control_diff_temp = 0.0 threshold = 0.02 # terminate the control once the arm moved to the desired joint space within the threshold while control_diff_record > threshold: pub.publish(command) for x, y in zip(self.jointAngles, command.acceleration): control_diff_temp = abs(x - y) + control_diff_temp control_diff_record = control_diff_temp control_diff_temp = 0.0 rate.sleep() rospy.loginfo( ">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def __init__(self): rospy.loginfo("Creating IK Controller class") # setup flags and useful vars: self.running_flag = False self.freq = rospy.get_param("~freq", FREQ) self.arm = rospy.get_param("~arm", ARM) self.base = rospy.get_param("~base", BASE) self.target = rospy.get_param("~target", TARGET) self.step_scale = rospy.get_param("~scale", STEP_SCALE) self.tolerance = rospy.get_param("~tolerance", TOLERANCE) self.damping = rospy.get_param("~damping", DAMPING) self.center_x = rospy.get_param("~center_x", CENTER_X) self.center_y = rospy.get_param("~center_y", CENTER_Y) self.center_z = rospy.get_param("~center_z", CENTER_Z) self.range_x = rospy.get_param("~range_x", RANGE_X) self.range_y = rospy.get_param("~range_y", RANGE_Y) self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT) with cl.suppress_stdout_stderr(): self.urdf = URDF.from_parameter_server() self.kin = KDLKinematics(self.urdf, "base", "%s_hand"%self.arm) self.q = np.zeros(7) self.q_sim = np.zeros(7) self.limb_interface = intera_interface.Limb() self.center_pos() self.goal = np.array(self.kin.forward(self.q)) self.js = JointState() self.js.name = self.kin.get_joint_names() self.pose = PoseStamped() self.mutex = threading.Lock() self.joint_cmd = JointCommand() self.joint_cmd.names = self.kin.get_joint_names() self.joint_cmd.mode = 2 # create all services: self.toggle_server = rospy.Service("toggle_controller", SetBool, self.toggle_handler) self.reset_server = rospy.Service("reset", Empty, self.reset_handler) self.random_server = rospy.Service("random", Empty, self.random_handler) # create all subscribers, timers, and publishers: self.ref_sub = rospy.Subscriber("ref_pose", PoseStamped, self.refcb) self.actual_js_sub = rospy.Subscriber("robot/joint_states", JointState, self.actual_js_cb) self.js_pub = rospy.Publisher("joint_states", JointState, queue_size=3) self.pose_pub = rospy.Publisher("pose", PoseStamped, queue_size=3) self.joint_cmd_timeout_pub = rospy.Publisher("robot/limb/right/joint_command_timeout", Float64, queue_size=3) self.center_pos() self.joint_cmd_pub = rospy.Publisher("robot/limb/right/joint_command", JointCommand, queue_size=3) self.int_timer = rospy.Timer(rospy.Duration(1/float(self.freq)), self.ik_step_timercb) return
def __init__(self, config=None): """Initialize. See the parent class. """ super(FrankaPandaReal, self).__init__(config=config) if rospy.get_name() == '/unnamed': rospy.init_node('franka_panda') assert rospy.get_name() != '/unnamed', 'Must call rospy.init_node().' tic = time.time() rospy.loginfo('Initializing robot...') self._head = intera_interface.Head() self._display = intera_interface.HeadDisplay() self._lights = intera_interface.Lights() self._limb = intera_interface.Limb() self._joint_names = self._limb.joint_names() self.cmd = [] self._command_msg = JointCommand() self._command_msg.names = self._joint_names self._commanders = dict(velocity=None, torque=None) try: self._gripper = intera_interface.Gripper() self._has_gripper = True except Exception: self._has_gripper = False self._robot_enable = intera_interface.RobotEnable(True) self._params = intera_interface.RobotParams() self._motion_planning = self.config.MOTION_PLANNING if self._motion_planning == 'moveit': rospy.loginfo('Initializing moveit toolkit...') moveit_commander.roscpp_initialize(sys.argv) self._scene = moveit_commander.PlanningSceneInterface() self._group = moveit_commander.MoveGroupCommander('right_arm') toc = time.time() rospy.loginfo('Initialization finished after %.3f seconds.' % (toc - tic))
def __init__(self): # Messages to set the goal position self.seq = 1 self.dyn_point = Point() self.dyn_quat = Quaternion() self.goal_pose = PoseStamped() self.joint_cmd = JointCommand() self.joint_cmd.header.seq = self.seq self.joint_cmd.mode = JointCommand.POSITION_MODE self._joint_names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] self.joint_cmd.names = self._joint_names # Create a publisher to send joint position commands self.joint_publisher = rospy.Publisher( '/robot/limb/right/joint_command', JointCommand, queue_size=5) # Wait for subscribers to listen while self.joint_publisher.get_num_connections() == 0: pass
def __init__(self): # Init node rospy.init_node('sawyer_vel_ctrl') # tflistener Init self.tf_lis = tf.TransformListener() # Trajectory Subscriber rospy.Subscriber("/desired_trajectory", TransformStamped, self.ctrl_r) # Joint States Subscriber rospy.Subscriber("/robot/joint_states", JointState, self.js_store) # Control Message Publisher self.pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) ## Control Gains self.Kp = 2 * np.eye(6) # Proportional self.Ki = 0 * np.eye(6) # Intergral ## Robot Description self.B_list = s_des.Blist self.M = s_des.M # Set initial joint states to 0, may need to change in IRL use # ...but you might not self.cur_config = np.zeros(9) # Wait for actual joint_states to be stored by js_store() callback while np.sum(self.cur_config) is 0: rospy.sleep(0.1) # Joint Command Message Init self.joint_ctrl_msg = JointCommand() self.joint_ctrl_msg.names = ['right_j0', 'head_pan', 'right_j1', 'right_j2', \ 'right_j3', 'right_j4', 'right_j5', 'right_j6'] self.joint_ctrl_msg.mode = VELOCITY_MODE # Set 0 initial velocity at all joints self.joint_ctrl_msg.velocity = \ np.ndarray.tolist(np.zeros(len(self.joint_ctrl_msg.names))) self.it_count = 0 # Iteration count for debug self.int_err = np.zeros(6) # Integrated error 0 -> t
def move_move_no(limb, group, target, speed_ratio=None, accel_ratio=None, timeout=None): trajectory_publisher = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size = 1) JointCommandMessage = JointCommand() JointCommandMessage.mode = 4 if speed_ratio is None: speed_ratio = 0.3 if girigiri_aiiiiii: speed_ratio = 0.8 if accel_ratio is None: accel_ratio = 0.1 if girigiri_aiiiiii: accel_ratio = 0.2 plan = group.plan(target) rospy.sleep(1) JointCommandMessage.names = plan.joint_trajectory.joint_names start_time = rospy.get_time() position_array = [] velocity_array = [] acceleration_array = [] time_array = [] for point in plan.joint_trajectory.points: position = point.positions velocity = point.velocities acceleration = point.accelerations position_array.append(position) velocity_array.append(velocity) acceleration_array.append(acceleration) desire_time = point.time_from_start.to_sec() time_array.append(desire_time) s_array = time_array wp_array = position_array path = ta.SplineInterpolator(s_array, wp_array) s_sampled = np.linspace(0, time_array[len(time_array) - 1], 100) q_sampled = path.eval(s_sampled) # --------------------------- plotting the interpolator -------------------------- # plt.plot(s_sampled, q_sampled) # plt.hold(True) # plt.plot(time_array, position_array, 'kd') # plt.legend(["joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7"]) # plt.hold(False) # plt.show() # exit() # ---------------------------- end of plotting ---------------------------------- # Create velocity bounds, then velocity constraint object dof = 7 vlim_ = np.ones(dof) * 5 #* speed_ratio vlim = np.vstack((-vlim_, vlim_)).T # Create acceleration bounds, then acceleration constraint object alim_ = np.ones(dof) * 2 * accel_ratio alim = np.vstack((-alim_, alim_)).T pc_vel = constraint.JointVelocityConstraint(vlim) pc_acc = constraint.JointAccelerationConstraint( alim, discretization_scheme=constraint.DiscretizationType.Interpolation) # Setup a parametrization instance instance = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel') # Retime the trajectory, only this step is necessary. t0 = time.time() jnt_traj, aux_traj = instance.compute_trajectory(0, 0) print("TOPPRA Parameterization time: {:} secs".format(time.time() - t0)) ts_sample = np.linspace(0, jnt_traj.get_duration(), 800) position_output = jnt_traj.eval(ts_sample) velocity_output = jnt_traj.evald(ts_sample) acceleration_output = jnt_traj.evaldd(ts_sample) # --------------------------- plotting the algorithm output --------------------------- # plt.subplot(3,1,1) # plt.plot(ts_sample, position_output) # plt.subplot(3,1,2) # plt.plot(ts_sample, velocity_output) # plt.subplot(3,1,3) # plt.plot(ts_sample, acceleration_output) # plt.show() # --------------------------- end plot the algorithm output --------------------------- start_time = rospy.get_time() for i in range(len(ts_sample) - 1): JointCommandMessage.position = position_output[i] JointCommandMessage.velocity = velocity_output[i] JointCommandMessage.acceleration = acceleration_output[i] desire_time = ts_sample[i + 1] while (rospy.get_time() - start_time) < desire_time: trajectory_publisher.publish(JointCommandMessage) JointCommandMessage.position = position_output[-1] JointCommandMessage.velocity = velocity_output[-1] JointCommandMessage.acceleration = acceleration_output[-1] trajectory_publisher.publish(JointCommandMessage)
def enable2nextcycle(freq, time): en = True if 2 * np.pi / freq < time: en = False return en if __name__ == '__main__': rospy.init_node('motion_f_const') rate = rospy.Rate(500) robot = robot_control() time.sleep(1) command = JointCommand() command.mode = 1 command.names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] # Variables NUM_DATA = 500 joint1_en = False joint2_en = False joint3_en = False joint4_en = True joint5_en = False joint6_en = False joint7_en = False
def main(): try: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('avalos_limb_py', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group_name = 'right_arm' group = moveit_commander.MoveGroupCommander(group_name) #frecuency for Sawyer robot f = 100 alfa = 0.9 rate = rospy.Rate(f) # add gripper gripper = intera_interface.Gripper('right_gripper') gripper.calibrate() gripper.open() moveit_robot_state = RobotState() joint_state = JointState() joint_state.header = Header() joint_state.header.stamp = rospy.Time.now() joint_state.name = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] #Define topic pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) limb = Limb() limb.move_to_neutral() limb.move_to_joint_positions({"right_j6": 2}) group.clear_pose_targets() group.set_start_state_to_current_state() # We can get the joint values from the group and adjust some of the values: pose_goal = geometry_msgs.msg.Pose() # Orientation pose_goal.orientation.x = -1 pose_goal.orientation.y = 0.0 pose_goal.orientation.z = 0.0 pose_goal.orientation.w = 0.0 q0 = np.array([]) q1 = np.array([]) q2 = np.array([]) q3 = np.array([]) q4 = np.array([]) q5 = np.array([]) q6 = np.array([]) # Cartesian position - Carga '01' pose_goal.position.x = 0.758552 pose_goal.position.y = -0.3435 pose_goal.position.z = 0.25 group.set_pose_target(pose_goal) carga1 = group.plan().joint_trajectory.points n = len(carga1) joint_state.position = [ carga1[-1].positions[0], carga1[-1].positions[1], carga1[-1].positions[2], carga1[-1].positions[3], carga1[-1].positions[4], carga1[-1].positions[5], carga1[-1].positions[6] ] moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) tmp = np.array([]) if (n > 8): tmp = np.append(tmp, 0) k = int(math.sqrt(n) + 2) r = int((n - 1) / float(k)) for i in range(k): print i tmp = np.append(tmp, int(r * (i + 1) - 1)) tmp = np.append(tmp, n - 1) else: for i in range(n): print i tmp = np.append(tmp, i) print "tmp:", tmp for i in range(len(tmp)): q0 = np.append(q0, carga1[int(tmp[i])].positions[0]) q1 = np.append(q1, carga1[int(tmp[i])].positions[1]) q2 = np.append(q2, carga1[int(tmp[i])].positions[2]) q3 = np.append(q3, carga1[int(tmp[i])].positions[3]) q4 = np.append(q4, carga1[int(tmp[i])].positions[4]) q5 = np.append(q5, carga1[int(tmp[i])].positions[5]) q6 = np.append(q6, carga1[int(tmp[i])].positions[6]) print "q000", q0 # Cartesian position - Carga '00' #pose_goal.position.x = 0.85 #pose_goal.position.y = -0.4 pose_goal.position.z = -0.01 group.set_pose_target(pose_goal) carga0 = group.plan().joint_trajectory.points q0 = np.append(q0, carga0[-1].positions[0]) q1 = np.append(q1, carga0[-1].positions[1]) q2 = np.append(q2, carga0[-1].positions[2]) q3 = np.append(q3, carga0[-1].positions[3]) q4 = np.append(q4, carga0[-1].positions[4]) q5 = np.append(q5, carga0[-1].positions[5]) q6 = np.append(q6, carga0[-1].positions[6]) #''' q = np.array([q0, q1, q2, q3, q4, q5, q6]) print "q001", q0 m_time, t_min_tiempo = min_time(q) start = time.time() opt = Opt_avalos(q, f, 0.9) v_time = opt.full_time() j_1, v_1, a_1, jk_1 = generate_path_cub(q, v_time, f) ext_1 = len(j_1[0, :]) end = time.time() print('Process Time:', end - start) v_jk = sqrt(np.mean(np.square(jk_1))) print("Opt Time:", v_time) print("Min Time:", m_time) #print('Optimizacion:',opt.result()) max_v = np.amax(np.absolute(v_1)) max_ac = np.amax(np.absolute(a_1)) max_jk = np.amax(np.absolute(jk_1)) print "Max Velo:", max_v print "Max Acel:", max_ac print "Max Jerk:", max_jk #raw_input('Iniciar_CT_execute?') #Build message my_msg = JointCommand() # POSITION_MODE my_msg.mode = 4 my_msg.names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5" ] #,"right_j6"] print("Control por trayectoria iniciado.") #time.sleep(0.25) q0 = np.array([]) q1 = np.array([]) q2 = np.array([]) q3 = np.array([]) q4 = np.array([]) q5 = np.array([]) q6 = np.array([]) q0 = np.append(q0, carga0[-1].positions[0]) q1 = np.append(q1, carga0[-1].positions[1]) q2 = np.append(q2, carga0[-1].positions[2]) q3 = np.append(q3, carga0[-1].positions[3]) q4 = np.append(q4, carga0[-1].positions[4]) q5 = np.append(q5, carga0[-1].positions[5]) q6 = np.append(q6, carga0[-1].positions[6]) joint_state.position = [ carga1[-1].positions[0], carga1[-1].positions[1], carga1[-1].positions[2], carga1[-1].positions[3], carga1[-1].positions[4], carga1[-1].positions[5], carga1[-1].positions[6] ] moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) # Cartesian position - Base '01' pose_goal.position.x = 0.80791 pose_goal.position.y = 0.4461 pose_goal.position.z = 0.2501 group.set_pose_target(pose_goal) base1 = group.plan().joint_trajectory.points n = len(base1) joint_state.position = [ base1[-1].positions[0], base1[-1].positions[1], base1[-1].positions[2], base1[-1].positions[3], base1[-1].positions[4], base1[-1].positions[5], base1[-1].positions[6] ] moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) tmp = np.array([]) if (n > 14): tmp = np.append(tmp, 0) k = int(math.sqrt(n) + 3) r = int((n - 1) / float(k)) for i in range(k): print i tmp = np.append(tmp, int(r * (i + 1) - 1)) tmp = np.append(tmp, n - 1) else: for i in range(n): print i tmp = np.append(tmp, i) print "tmp:", tmp for i in range(len(tmp)): q0 = np.append(q0, base1[int(tmp[i])].positions[0]) q1 = np.append(q1, base1[int(tmp[i])].positions[1]) q2 = np.append(q2, base1[int(tmp[i])].positions[2]) q3 = np.append(q3, base1[int(tmp[i])].positions[3]) q4 = np.append(q4, base1[int(tmp[i])].positions[4]) q5 = np.append(q5, base1[int(tmp[i])].positions[5]) q6 = np.append(q6, base1[int(tmp[i])].positions[6]) print "q000", q0 # Cartesian position - Base '00' #pose_goal.position.x = 0.90 #pose_goal.position.y = 0.38 pose_goal.position.z = 0.01 group.set_pose_target(pose_goal) base0 = group.plan().joint_trajectory.points q0 = np.append(q0, base0[-1].positions[0]) q1 = np.append(q1, base0[-1].positions[1]) q2 = np.append(q2, base0[-1].positions[2]) q3 = np.append(q3, base0[-1].positions[3]) q4 = np.append(q4, base0[-1].positions[4]) q5 = np.append(q5, base0[-1].positions[5]) q6 = np.append(q6, base0[-1].positions[6]) q = np.array([q0, q1, q2, q3, q4, q5, q6]) print "q001", q0 #print q[0] #return 0 m_time, t_min_tiempo = min_time(q) start = time.time() opt = Opt_avalos(q, f, alfa) v_time = opt.full_time() j_2, v_2, a_2, jk_2 = generate_path_cub(q, v_time, f) ext_2 = len(j_2[0, :]) end = time.time() print('Process Time:', end - start) #save_matrix(j,"data_p.txt",f) #save_matrix(v,"data_v.txt",f) #save_matrix(a,"data_a.txt",f) #save_matrix(jk,"data_y.txt",f) v_jk = sqrt(np.mean(np.square(jk_2))) print("Opt Time:", v_time) print("Min Time:", m_time) #print('Optimizacion:',opt.result()) max_v = np.amax(np.absolute(v_2)) max_ac = np.amax(np.absolute(a_2)) max_jk = np.amax(np.absolute(jk_2)) print "Max Velo:", max_v print "Max Acel:", max_ac print "Max Jerk:", max_jk q0 = np.array([]) q1 = np.array([]) q2 = np.array([]) q3 = np.array([]) q4 = np.array([]) q5 = np.array([]) q6 = np.array([]) q0 = np.append(q0, base0[-1].positions[0]) q1 = np.append(q1, base0[-1].positions[1]) q2 = np.append(q2, base0[-1].positions[2]) q3 = np.append(q3, base0[-1].positions[3]) q4 = np.append(q4, base0[-1].positions[4]) q5 = np.append(q5, base0[-1].positions[5]) q6 = np.append(q6, base0[-1].positions[6]) # Cartesian position - Carga '01' pose_goal.position.x = 0.7708552 pose_goal.position.y = -0.394135 pose_goal.position.z = 0.24 group.set_pose_target(pose_goal) carga1 = group.plan().joint_trajectory.points n = len(carga1) tmp = np.array([]) if (n > 10): tmp = np.append(tmp, 0) k = int(math.sqrt(n) + 2) r = int((n - 1) / float(k)) for i in range(k): print i tmp = np.append(tmp, int(r * (i + 1) - 1)) tmp = np.append(tmp, n - 1) else: for i in range(n): print i tmp = np.append(tmp, i) print "tmp:", tmp for i in range(len(tmp)): q0 = np.append(q0, carga1[int(tmp[i])].positions[0]) q1 = np.append(q1, carga1[int(tmp[i])].positions[1]) q2 = np.append(q2, carga1[int(tmp[i])].positions[2]) q3 = np.append(q3, carga1[int(tmp[i])].positions[3]) q4 = np.append(q4, carga1[int(tmp[i])].positions[4]) q5 = np.append(q5, carga1[int(tmp[i])].positions[5]) q6 = np.append(q6, carga1[int(tmp[i])].positions[6]) q = np.array([q0, q1, q2, q3, q4, q5, q6]) print "q001", q0 #print q[0] #return 0 m_time, t_min_tiempo = min_time(q) start = time.time() opt = Opt_avalos(q, f, 0.8) v_time = opt.full_time() j_3, v_3, a_3, jk_3 = generate_path_cub(q, v_time, f) ext_3 = len(j_3[0, :]) end = time.time() print('Process Time:', end - start) #save_matrix(j,"data_p.txt",f) #save_matrix(v,"data_v.txt",f) #save_matrix(a,"data_a.txt",f) #save_matrix(jk,"data_y.txt",f) v_jk = sqrt(np.mean(np.square(jk_3))) print("Opt Time:", v_time) print("Min Time:", m_time) #print('Optimizacion:',opt.result()) max_v = np.amax(np.absolute(v_3)) max_ac = np.amax(np.absolute(a_3)) max_jk = np.amax(np.absolute(jk_3)) print "Max Velo:", max_v print "Max Acel:", max_ac print "Max Jerk:", max_jk raw_input('Iniciar_CT_execute?') #Build message for n in xrange(ext_1): my_msg.position = [ j_1[0][n], j_1[1][n], j_1[2][n], j_1[3][n], j_1[4][n], j_1[5][n] ] #,j_1[6][n]] my_msg.velocity = [ v_1[0][n], v_1[1][n], v_1[2][n], v_1[3][n], v_1[4][n], v_1[5][n] ] #,v_1[6][n]] my_msg.acceleration = [ a_1[0][n], a_1[1][n], a_1[2][n], a_1[3][n], a_1[4][n], a_1[5][n] ] #,a_1[6][n]] pub.publish(my_msg) rate.sleep() print("Control por trayectoria terminado.") time.sleep(0.25) gripper.close() print("Control por trayectoria iniciado.") #time.sleep(0.25) for n in xrange(ext_2): my_msg.position = [ j_2[0][n], j_2[1][n], j_2[2][n], j_2[3][n], j_2[4][n], j_2[5][n] ] #,j_2[6][n]] my_msg.velocity = [ v_2[0][n], v_2[1][n], v_2[2][n], v_2[3][n], v_2[4][n], v_2[5][n] ] #,v_2[6][n]] my_msg.acceleration = [ a_2[0][n], a_2[1][n], a_2[2][n], a_2[3][n], a_2[4][n], a_2[5][n] ] #,a_2[6][n]] pub.publish(my_msg) rate.sleep() print("Control por trayectoria terminado.") gripper.open() time.sleep(0.5) #data.writeoff() print("Programa terminado.") for n in xrange(ext_3): my_msg.position = [ j_3[0][n], j_3[1][n], j_3[2][n], j_3[3][n], j_3[4][n], j_3[5][n] ] #,j_3[6][n]] my_msg.velocity = [ v_3[0][n], v_3[1][n], v_3[2][n], v_3[3][n], v_3[4][n], v_3[5][n] ] #,v_3[6][n]] my_msg.acceleration = [ a_3[0][n], a_3[1][n], a_3[2][n], a_3[3][n], a_3[4][n], a_3[5][n] ] #,a_3[6][n]] pub.publish(my_msg) rate.sleep() print("Control por trayectoria terminado.") gripper.open() #data.writeoff() print("Programa terminado.") except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user.')
self.pub_joints.publish(_robot) time.sleep(0.01) def effort_sinpercycle(start_time): joint_torq_desired = 0.1 #0.001*np.sin(freq*(time.time() - start_time))# + np.random.randn(1)*0.1 return joint_torq_desired if __name__ == '__main__': rospy.init_node('motion_f_const') rate = rospy.Rate(100) robot = robot_control() time.sleep(1) command = JointCommand() command.mode = 3 command.names = ['right_j3'] #model = load_model('model_zero.h5') # Variables NUM_DATA = 2000 joint1_en = False joint2_en = False joint3_en = False joint4_en = True joint5_en = False joint6_en = False joint7_en = False amplitude = 0.1 DIRECTORY = '../data/sawyer/temp/' FILENAME = DIRECTORY + 'torque_test.csv'
def __init__(self, mode="real"): """ Arguments mode - "real" for the real robot (default), "sim" for the gazebo simulation """ # Lengths for the links (in meters) self.l0 = 0.081; self.h = 0.317; self.l1 = 0.1925; self.l2 = 0.4; self.l3 = 0.1685; self.l4 = 0.4; self.l5 = 0.1363; self.l6 = 0.13375 # Memory allocation self.Jp = np.zeros((3,7)) self.J = np.zeros((7,7)) # Joint limits self.qmin = (-3.05, -3.81, -3.04, -3.04, -2.98, -2.98, -4.71) self.qmax = ( 3.05, 2.27, 3.04, 3.04, 2.98, 2.98, 4.71) # Topic that contains the sensed joint configuration rospy.Subscriber("/robot/joint_states", JointState, self._readJoints) # Sensed joint configuration self.jstate_ = 7*[0.,] # Markers for current and desired pose/position of the wrist self.wrist_current_frame = FrameMarker() self.wrist_des_frame = FrameMarker(0.5) self.wrist_current_ball = BallMarker(color['GREEN']) self.wrist_des_ball = BallMarker(color['RED']) # Initialize the markers at zero x0 = np.array([0., 0., 0., 1., 0., 0., 0.]) self.wrist_current_frame.setPose(x0) self.wrist_des_frame.setPose(x0) # Store the mode self.mode = mode # For the real robot if (self.mode=="real"): ns = "/robot/limb/right/" jtopic = ns + "joint_command" self.pubjs = rospy.Publisher(jtopic, JointCommand, queue_size=10) self.jcommand = JointCommand() self.jcommand.mode = 1 self.jcommand.names = ["right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6"] self.set_joints = self._publish_real # For the Gazebo simulation elif (self.mode=="sim"): ns = "/robot/right_joint_position_controller/joints" topic0 = ns + "/right_j0_controller/command" topic1 = ns + "/right_j1_controller/command" topic2 = ns + "/right_j2_controller/command" topic3 = ns + "/right_j3_controller/command" topic4 = ns + "/right_j4_controller/command" topic5 = ns + "/right_j5_controller/command" topic6 = ns + "/right_j6_controller/command" self.pub0 = rospy.Publisher(topic0, Float64, queue_size=10) self.pub1 = rospy.Publisher(topic1, Float64, queue_size=10) self.pub2 = rospy.Publisher(topic2, Float64, queue_size=10) self.pub3 = rospy.Publisher(topic3, Float64, queue_size=10) self.pub4 = rospy.Publisher(topic4, Float64, queue_size=10) self.pub5 = rospy.Publisher(topic5, Float64, queue_size=10) self.pub6 = rospy.Publisher(topic6, Float64, queue_size=10) self.set_joints = self._publish_sim else: rospy.logerr("Joint Interface: invalid option for mode")
def __init__(self, limb="right", synchronous_pub=False): """ Constructor. @type limb: str @param limb: limb to interface @type synchronous_pub: bool @param synchronous_pub: designates the JointCommand Publisher as Synchronous if True and Asynchronous if False. Synchronous Publishing means that all joint_commands publishing to the robot's joints will block until the message has been serialized into a buffer and that buffer has been written to the transport of every current Subscriber. This yields predicable and consistent timing of messages being delivered from this Publisher. However, when using this mode, it is possible for a blocking Subscriber to prevent the joint_command functions from exiting. Unless you need exact JointCommand timing, default to Asynchronous Publishing (False). For more information about Synchronous Publishing see: http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers#queue_size:_publish.28.29_behavior_and_queuing """ params = RobotParams() limb_names = params.get_limb_names() if limb not in limb_names: rospy.logerr("Cannot detect limb {0} on this robot." " Valid limbs are {1}. Exiting Limb.init().".format( limb, limb_names)) return joint_names = params.get_joint_names(limb) if not joint_names: rospy.logerr("Cannot detect joint names for limb {0} on this " "robot. Exiting Limb.init().".format(limb)) return self.name = limb self._joint_angle = dict() self._joint_velocity = dict() self._joint_effort = dict() self._cartesian_pose = dict() self._cartesian_velocity = dict() self._cartesian_effort = dict() self._joint_names = joint_names self._collision_state = False self._tip_states = None ns = '/robot/limb/' + limb + '/' self._command_msg = JointCommand() self._pub_speed_ratio = rospy.Publisher( ns + 'set_speed_ratio', Float64, latch=True, queue_size=10) queue_size = None if synchronous_pub else 1 with warnings.catch_warnings(): warnings.simplefilter("ignore") self._pub_joint_cmd = rospy.Publisher( ns + 'joint_command', JointCommand, tcp_nodelay=True, queue_size=queue_size) self._pub_joint_cmd_timeout = rospy.Publisher( ns + 'joint_command_timeout', Float64, latch=True, queue_size=10) _cartesian_state_sub = rospy.Subscriber( ns + 'endpoint_state', EndpointState, self._on_endpoint_states, queue_size=1, tcp_nodelay=True) _tip_states_sub = rospy.Subscriber( ns + 'tip_states', EndpointStates, self._on_tip_states, queue_size=1, tcp_nodelay=True) _collision_state_sub = rospy.Subscriber( ns + 'collision_detection_state', CollisionDetectionState, self._on_collision_state, queue_size=1, tcp_nodelay=True) joint_state_topic = '/robot/joint_states' _joint_state_sub = rospy.Subscriber( joint_state_topic, JointState, self._on_joint_states, queue_size=1, tcp_nodelay=True) ns_pkn = "ExternalTools/" + limb + "/PositionKinematicsNode/" self._iksvc = rospy.ServiceProxy(ns_pkn + 'IKService', SolvePositionIK) self._fksvc = rospy.ServiceProxy(ns_pkn + 'FKService', SolvePositionFK) rospy.wait_for_service(ns_pkn + 'IKService', 5.0) rospy.wait_for_service(ns_pkn + 'FKService', 5.0) err_msg = ("%s limb init failed to get current joint_states " "from %s") % (self.name.capitalize(), joint_state_topic) intera_dataflow.wait_for(lambda: len(list(self._joint_angle.keys())) > 0, timeout_msg=err_msg, timeout=5.0) err_msg = ("%s limb init failed to get current endpoint_state " "from %s") % (self.name.capitalize(), ns + 'endpoint_state') intera_dataflow.wait_for(lambda: len(list(self._cartesian_pose.keys())) > 0, timeout_msg=err_msg, timeout=5.0) err_msg = ("%s limb init failed to get current tip_states " "from %s") % (self.name.capitalize(), ns + 'tip_states') intera_dataflow.wait_for(lambda: self._tip_states is not None, timeout_msg=err_msg, timeout=5.0)
def main(): try: rospy.init_node('avalos_limb_py') #Frecuency for Sawyer robot f = 100 rate = rospy.Rate(f) #Define topic pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) # Class to record data = Data() #Class limb to acces information sawyer limb = Limb() #Initial position limb.move_to_neutral() time.sleep(3) # Position init initial = limb.joint_angles() print "Posicion inicial terminada" #begin to record data data.writeon("cin_directa.txt") time.sleep(0.5) limb.move_to_joint_positions({ "right_j6": 0.0, "right_j5": 0.0, "right_j4": 0.0, "right_j3": 0.0, "right_j2": 0.0, "right_j1": 0.0, "right_j0": 0.0 }) limb.move_to_joint_positions({ "right_j6": initial["right_j6"], "right_j5": initial["right_j5"], "right_j4": initial["right_j4"], "right_j3": initial["right_j3"], "right_j2": initial["right_j2"], "right_j1": initial["right_j1"], "right_j0": initial["right_j0"] }) time.sleep(1) data.writeoff() print "FINISH" initial = limb.joint_angles() p0 = np.array([ initial["right_j0"], initial["right_j1"], initial["right_j2"], initial["right_j3"], initial["right_j4"], initial["right_j5"], initial["right_j6"] ]) # Posiition end p1 = [0, 0, 0, 0, 0, 0, 0] p2 = p0 # Knost vector time. We assum the process will take 10 sec k = np.array([0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1]) tiempo_estimado = 7 k_t = tiempo_estimado * k k_pt = np.array([k_t[0], k_t[5], k_t[-1]]) # Set inperpolation in linear form k_j0 = sp.interpolate.interp1d(k_pt, [p0[0], p1[0], p2[0]], kind='linear')(k_t) k_j1 = sp.interpolate.interp1d(k_pt, [p0[1], p1[1], p2[1]], kind='linear')(k_t) k_j2 = sp.interpolate.interp1d(k_pt, [p0[2], p1[2], p2[2]], kind='linear')(k_t) k_j3 = sp.interpolate.interp1d(k_pt, [p0[3], p1[3], p2[3]], kind='linear')(k_t) k_j4 = sp.interpolate.interp1d(k_pt, [p0[4], p1[4], p2[4]], kind='linear')(k_t) k_j5 = sp.interpolate.interp1d(k_pt, [p0[5], p1[5], p2[5]], kind='linear')(k_t) k_j6 = sp.interpolate.interp1d(k_pt, [p0[6], p1[6], p2[6]], kind='linear')(k_t) # Obtain all point following the interpolated points j0 = path_simple_cub_v0(k_j0, k_t, f) j1 = path_simple_cub_v0(k_j1, k_t, f) j2 = path_simple_cub_v0(k_j2, k_t, f) j3 = path_simple_cub_v0(k_j3, k_t, f) j4 = path_simple_cub_v0(k_j4, k_t, f) j5 = path_simple_cub_v0(k_j5, k_t, f) j6 = path_simple_cub_v0(k_j6, k_t, f) l = len(j0) print "l" print l # Vector for velocity v_j0 = np.zeros(l) v_j1 = np.zeros(l) v_j2 = np.zeros(l) v_j3 = np.zeros(l) v_j4 = np.zeros(l) v_j5 = np.zeros(l) v_j6 = np.zeros(l) # Vector for acceleration a_j0 = np.zeros(l) a_j1 = np.zeros(l) a_j2 = np.zeros(l) a_j3 = np.zeros(l) a_j4 = np.zeros(l) a_j5 = np.zeros(l) a_j6 = np.zeros(l) # Vector for jerk jk_j0 = np.zeros(l) jk_j1 = np.zeros(l) jk_j2 = np.zeros(l) jk_j3 = np.zeros(l) jk_j4 = np.zeros(l) jk_j5 = np.zeros(l) jk_j6 = np.zeros(l) for i in xrange(l - 1): v_j0[i] = (j0[i + 1] - j0[i]) * f v_j1[i] = (j1[i + 1] - j1[i]) * f v_j2[i] = (j2[i + 1] - j2[i]) * f v_j3[i] = (j3[i + 1] - j3[i]) * f v_j4[i] = (j4[i + 1] - j4[i]) * f v_j5[i] = (j5[i + 1] - j5[i]) * f v_j6[i] = (j6[i + 1] - j6[i]) * f v_j0[-1] = v_j0[-2] v_j1[-1] = v_j0[-2] v_j2[-1] = v_j0[-2] v_j3[-1] = v_j0[-2] v_j4[-1] = v_j0[-2] v_j5[-1] = v_j0[-2] v_j6[-1] = v_j0[-2] for i in xrange(l - 1): a_j0[i] = (v_j0[i + 1] - v_j0[i]) * f a_j1[i] = (v_j1[i + 1] - v_j1[i]) * f a_j2[i] = (v_j2[i + 1] - v_j2[i]) * f a_j3[i] = (v_j3[i + 1] - v_j3[i]) * f a_j4[i] = (v_j4[i + 1] - v_j4[i]) * f a_j5[i] = (v_j5[i + 1] - v_j5[i]) * f a_j6[i] = (v_j6[i + 1] - v_j6[i]) * f a_j0[-2] = a_j0[-3] a_j1[-2] = a_j1[-3] a_j2[-2] = a_j2[-3] a_j3[-2] = a_j3[-3] a_j4[-2] = a_j4[-3] a_j5[-2] = a_j5[-3] a_j6[-2] = a_j6[-3] a_j0[-1] = a_j0[-2] a_j1[-1] = a_j1[-2] a_j2[-1] = a_j2[-2] a_j3[-1] = a_j3[-2] a_j4[-1] = a_j4[-2] a_j5[-1] = a_j5[-2] a_j6[-1] = a_j6[-2] for i in xrange(l - 1): jk_j0[i] = (a_j0[i + 1] - a_j0[i]) * f jk_j1[i] = (a_j1[i + 1] - a_j1[i]) * f jk_j2[i] = (a_j2[i + 1] - a_j2[i]) * f jk_j3[i] = (a_j3[i + 1] - a_j3[i]) * f jk_j4[i] = (a_j4[i + 1] - a_j4[i]) * f jk_j5[i] = (a_j5[i + 1] - a_j5[i]) * f jk_j6[i] = (a_j6[i + 1] - a_j6[i]) * f jk_j0[-3] = jk_j0[-4] jk_j1[-3] = jk_j1[-4] jk_j2[-3] = jk_j2[-4] jk_j3[-3] = jk_j3[-4] jk_j4[-3] = jk_j4[-4] jk_j5[-3] = jk_j5[-4] jk_j6[-3] = jk_j6[-4] jk_j0[-2] = jk_j0[-3] jk_j1[-2] = jk_j1[-3] jk_j2[-2] = jk_j2[-3] jk_j3[-2] = jk_j3[-3] jk_j4[-2] = jk_j4[-3] jk_j5[-2] = jk_j5[-3] jk_j6[-2] = jk_j6[-3] jk_j0[-1] = jk_j0[-2] jk_j1[-1] = jk_j1[-2] jk_j2[-1] = jk_j2[-2] jk_j3[-1] = jk_j3[-2] jk_j4[-1] = jk_j4[-2] jk_j5[-1] = jk_j5[-2] jk_j6[-1] = jk_j6[-2] j = np.array([j0, j1, j2, j3, j4, j5, j6]) v = np.array([v_j0, v_j1, v_j2, v_j3, v_j4, v_j5, v_j6]) a = np.array([a_j0, a_j1, a_j2, a_j3, a_j4, a_j5, a_j6]) jk = np.array([jk_j0, jk_j1, jk_j2, jk_j3, jk_j4, jk_j5, jk_j6]) save_matrix(j, "data_p.txt", f) save_matrix(v, "data_v.txt", f) save_matrix(a, "data_a.txt", f) save_matrix(jk, "data_y.txt", f) #Build message my_msg = JointCommand() # POSITION_MODE my_msg.mode = 4 my_msg.names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] data.writeon("cin_trayectoria.txt") time.sleep(0.5) for i in xrange(l): my_msg.position = [j0[i], j1[i], j2[i], j3[i], j4[i], j5[i], j6[i]] my_msg.velocity = [ v_j0[i], v_j1[i], v_j2[i], v_j3[i], v_j4[i], v_j5[i], v_j6[i] ] my_msg.acceleration = [ a_j0[i], a_j1[i], a_j2[i], a_j3[i], a_j4[i], a_j5[i], a_j6[i] ] pub.publish(my_msg) rate.sleep() time.sleep(1) data.writeoff() print "Programa terminado " except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user.')
def move_to_ja(self, waypoints, duration=1.5): """ :param waypoints: List of joint angle arrays. If len(waypoints) == 1: then go directly to point. Otherwise: take trajectory that ends at waypoints[-1] and passes through each intermediate waypoint :param duration: Total time trajectory will take before ending """ self._try_enable() jointnames = self._limb.joint_names() prev_joint = np.array([self._limb.joint_angle(j) for j in jointnames]) waypoints = np.array([prev_joint] + waypoints) spline = CSpline(waypoints, duration) start_time = rospy.get_time() # in seconds finish_time = start_time + duration # in seconds time = rospy.get_time() while time < finish_time: pos, velocity, acceleration = spline.get(time - start_time) command = JointCommand() command.mode = JointCommand.POSITION_MODE command.names = jointnames command.position = pos command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag) command.acceleration = np.clip(acceleration, -max_accel_mag, max_accel_mag) self._cmd_publisher.publish(command) self._control_rate.sleep() time = rospy.get_time() for i in xrange(10): command = JointCommand() command.mode = JointCommand.POSITION_MODE command.names = jointnames command.position = waypoints[-1] self._cmd_publisher.publish(command) self._control_rate.sleep()
def set_move(): F = 100 #Frecuencia de envio rate = rospy.Rate(F) # hz limb = intera_interface.Limb('right') limb.move_to_neutral() ik_service_client_full([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0]) print "Posicion neutral terminada" time.sleep(1) p1 = np.array([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0]) p2 = np.array([0.662, 0.200, 0.290, 0.717, 0, 0.717, 0]) p3 = np.array([0.662, -0.150, 0.290, 0.717, 0, 0.717, 0]) p4 = np.array([0.662, -0.450, -0.040, 0.717, 0, 0.717, 0]) p5 = np.array([0.662, -0.150, 0.290, 0.717, 0, 0.717, 0]) p6 = np.array([0.662, 0.200, 0.290, 0.717, 0, 0.717, 0]) p7 = np.array([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0]) data = Data() data.writeon("IK_data.txt") # Control de posicion por cinematica inversa time.sleep(0.75) [succes, q1] = ik_service_client_full(p1) [succes, q2] = ik_service_client_full(p2) [succes, q3] = ik_service_client_full(p3) [succes, q4] = ik_service_client_full(p4) [succes, q5] = ik_service_client_full(p5) [succes, q6] = ik_service_client_full(p6) [succes, q7] = ik_service_client_full(p7) time.sleep(0.75) data.writeoff() print "Control de Posicion Cinematica Inversa terminada" names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) # Control de trayectoria cinematica inversa q = np.matrix([q1, q2, q3, q4, q5, q6, q7]) print q [t, t_rec] = min_time(q, F) k = 1.25 #Factor de Tiempo t_points = [k * x for x in t] print "t_points", t_points #print "Tiempo de recorrido",t_rec,"s" [j, ext] = generate_path_cub(q, t_points, F) [v, ext] = generate_vel(j, F) [a, ext] = generate_acel(v, F) [jk, v_jk, ext] = generate_jerk(a, F) v_t = 6 * (t_points[-1] - t_points[0]) raw_input('Iniciar?') print "Valor tiempo: ", v_t, "Valor jerk", v_jk save_matrix(j, "save_data_p.txt", F) save_matrix(v, "save_data_v.txt", F) save_matrix(a, "save_data_a.txt", F) save_matrix(jk, "save_data_y.txt", F) ik_service_client_full([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0]) time.sleep(1) my_msg = JointCommand() my_msg.mode = 4 my_msg.names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] data.writeon("S_data.txt") time.sleep(0.75) if (my_msg.mode == 4): for n in range(ext): my_msg.position = [ j[0][n], j[1][n], j[2][n], j[3][n], j[4][n], j[5][n], j[6][n] ] my_msg.velocity = [ v[0][n], v[1][n], v[2][n], v[3][n], v[4][n], v[5][n], v[6][n] ] my_msg.acceleration = [ a[0][n], a[1][n], a[2][n], a[3][n], a[4][n], a[5][n], a[6][n] ] pub.publish(my_msg) rate.sleep() time.sleep(0.75) return True
def move_with_impedance(self, waypoints, duration=1.5): """ Moves from curent position to final position while hitting waypoints :param waypoints: List of arrays containing waypoint joint angles :param duration: trajectory duration """ self._try_enable() jointnames = self.limb.joint_names() prev_joint = np.array([self.limb.joint_angle(j) for j in jointnames]) waypoints = np.array([prev_joint] + waypoints) spline = CSpline(waypoints, duration) start_time = rospy.get_time() # in seconds finish_time = start_time + duration # in seconds time = rospy.get_time() while time < finish_time: pos, velocity, acceleration = spline.get(time - start_time) command = JointCommand() command.mode = JointCommand.POSITION_MODE command.names = jointnames command.position = pos command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag) command.acceleration = np.clip(acceleration, -max_accel_mag, max_accel_mag) self._cmd_publisher.publish(command) self.control_rate.sleep() time = rospy.get_time() for i in xrange(10): command = JointCommand() command.mode = JointCommand.POSITION_MODE command.names = jointnames command.position = waypoints[-1] self._cmd_publisher.publish(command) self.control_rate.sleep()