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()
