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 _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 _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)
velocity7 = copy.copy(robot.vel_data[7]) else: joint7_pos = 0. effort7 = 0. joint7_desired = 0. velocity7 = 0. """ command.effort = [joint1_desired, joint2_desired, joint3_desired, joint4_desired, joint5_desired, joint6_desired, joint7_desired] robot.pub_joints.publish(command) df_tmp = pd.DataFrame([[joint1_pos, joint2_pos, joint3_pos, joint4_pos, joint5_pos, joint6_pos, joint7_pos, velocity1, velocity2, velocity3, velocity4, velocity5, velocity6, velocity7, joint1_desired, joint2_desired, joint3_desired, joint4_desired, joint5_desired, joint6_desired, joint7_desired, effort1, effort2, effort3, effort4, effort5, effort6, effort7]], columns=column_index) df = df.append(df_tmp, ignore_index=True) rate.sleep() print itera """ command.effort = [joint4_desired] #robot.pub_joints.publish(command) df_tmp = pd.DataFrame([[ joint1_pos, joint2_pos, joint3_pos, joint4_pos, joint5_pos, joint6_pos, joint7_pos, velocity1, velocity2, velocity3, velocity4, velocity5, velocity6, velocity7, 0., 0., 0., joint4_desired, 0., 0., 0., effort1, effort2, effort3, effort4, effort5, effort6, effort7 ]], columns=column_index) df = df.append(df_tmp, ignore_index=True) rate.sleep() print joint4_desired df.to_csv(FILENAME)
print joint7_desired print 'Inicia en' time.sleep(0.5) print '3' time.sleep(1) print '2' time.sleep(1) print '1' time.sleep(1) for itera in range(NUM_DATA): joint4_desired = effort_sinpercycle(start) #robot.pub_joint.publish(joint4_desired - robot.joint4_pos_zero) joint4_pos = copy.copy(robot.joint_data[4]) effort4 = copy.copy(robot.effort_data[4]) velocity4 = copy.copy(robot.vel_data[4]) command.effort = [ joint1_desired, joint2_desired, joint3_desired, joint4_desired, joint5_desired, joint6_desired, joint7_desired ] robot.pub_joints.publish(command) df_tmp = pd.DataFrame( [[joint4_pos, velocity4, joint4_desired, effort4]], columns=column_index) df = df.append(df_tmp, ignore_index=True) rate.sleep() print itera df.to_csv(FILENAME)
def _ctl_loop(self): rate = rospy.Rate(self._ctl_freq) # # trajectory_options = TrajectoryOptions() # trajectory_options.interaction_control = False # trajectory_options.interpolation_type = TrajectoryOptions.JOINT last_vel = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) last_acc = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) rec_data = [] rec_seq = 0 def compute_vel(measured, desired): tol = 0.008 mp = measured[0, :] dp = desired[0, :] mv = measured[1, :] dv = desired[1, :] cycle = self._des_provider[ self._active_des_provider]._cycles_since_active if cycle == 1: max_steps = (3.5 * np.abs(dp - mp) * self._ctl_freq + 50).astype(np.int) max_steps[np.abs(dp - mp) < tol] = 1 self._des_provider[ self._active_des_provider]._HACK_MAX_STEPS = max_steps else: max_steps = self._des_provider[ self._active_des_provider]._HACK_MAX_STEPS # max_steps = 500 steps = np.minimum(cycle, max_steps).astype(np.float64) weights = (steps / max_steps) dgain = 0.0 pgain = 4.0 ep = (mp * (1.0 - weights) + weights * dp) - mp ep[np.abs(dp - mp) < tol] = 0.0 ev = dv - (mv * (1.0 - weights) + weights * dv) sv = pgain * ep + dgain * ev sv = np.maximum(np.minimum(sv, 4.0), -4.0) return sv while not rospy.is_shutdown(): des_joint_state, des_gripper_state = self.get_active_desired_states( state_ids=('joint', 'gripper')) self.run_cycle() timestamp = rospy.Time.now() if des_joint_state is not None: MODE = self._des_provider[self._active_des_provider]._HACK_MODE joint_msg = JointCommand() joint_msg.header.stamp = timestamp joint_msg.names = self._joint_names acc = (self._joint_state[1, :] - last_vel) / 0.01 jerk = (acc - last_acc) / 0.01 vel = compute_vel(self._joint_state, des_joint_state) if MODE == JointCommand.VELOCITY_MODE: # if np.any(np.abs(vel[-1]) > 10.0): # print("pos msr:{} des:{}\tvel msr:{} des:{}\tset: {} acc: {} jerk: {}".format(self._joint_state[0,-1],des_joint_state[0,-1],self._joint_state[1,-1],des_joint_state[1,-1],vel[-1],acc[-1], jerk[-1])) if self.HACK_do_print: # print("pos msr:{} des:{}\tvel msr:{} des:{}\tset: {} acc: {} jerk: {}".format(self._joint_state[0,-1],des_joint_state[0,-1],self._joint_state[1,-1],des_joint_state[1,-1],vel[-1],acc[-1], jerk[-1])) with np.printoptions(precision=6, suppress=True, floatmode='fixed', sign=' '): # print("pm:{},\npd:{},\nvm:{},\nvd:{},\nvs:{},".format(np.array2string(self._joint_state[0,:]),np.array2string(des_joint_state[0,:]),np.array2string(self._joint_state[1,:]),np.array2string(des_joint_state[1,:]),np.array2string(vel))) # print(np.array2string(np.array([self._joint_state[0,:], des_joint_state[0,:], self._joint_state[1,:], des_joint_state[1,:], vel]))) rec_data.append( np.array([ self._joint_state[0, :], des_joint_state[0, :], self._joint_state[1, :], des_joint_state[1, :], vel ])) joint_msg.mode = JointCommand.VELOCITY_MODE joint_msg.position = [] joint_msg.velocity = vel joint_msg.acceleration = [] joint_msg.effort = [] elif MODE == JointCommand.POSITION_MODE: print("NOOOOO:(") joint_msg.mode = JointCommand.POSITION_MODE joint_msg.position = des_joint_state[0, :] joint_msg.velocity = [] joint_msg.acceleration = [] joint_msg.effort = [] self._joint_command_pub.publish(joint_msg) if self._joint_state is not None and np.all( np.abs(self._joint_state[0, :] - des_joint_state[0, :]) < 0.008): self.set_reached() if self.HACK_do_print: self.HACK_do_print = False # np.save('data_{}_{:03d}'.format(self._name,rec_seq),np.array(rec_data)) rec_seq += 1 rec_data = [] last_vel = self._joint_state[1, :] last_acc = acc if des_gripper_state is not None: gripper_msg = IOComponentCommand() gripper_msg.time = timestamp gripper_msg.op = "set" gripper_msg.args = '{"signals": {"position_m": {"data": [' + str( des_gripper_state) + '], "format": {"type": "float"}}}}' self._gripper_command_pub.publish(gripper_msg) rate.sleep()