class JointForceViz:
def __init__(self, arm):
self.arm = arm
self.vis_pub = rospy.Publisher("force_torque_markers", Marker)
if arm == 'r':
self.arm_inds = JOINT_STATE_INDS_R
else:
self.arm_inds = JOINT_STATE_INDS_L
model = urdf.create_model_from_file("/etc/ros/diamondback/urdf/robot.xml")
tree = kdlp.tree_from_urdf_model(model)
chain = tree.getChain("torso_lift_link", "r_gripper_tool_frame")
link_dict = {0:0, 1:1, 2:2, 3:3, 4:4, 5:5, 6:6, 7:11}
self.kinematics = KDLArmKinematics(chain=chain, link_dict=link_dict)
self.colors = [ColorRGBA(1., 0., 0., 1.), ColorRGBA(0., 1., 0., 1.)]
rospy.Subscriber("joint_states", JointState, self.joint_states_cb)
self.last_CI = np.zeros((6, 6))
self.last_q_pos = np.zeros((7, 1))
def joint_states_cb(self, msg):
q_pos = np.mat([msg.position[i] for i in self.arm_inds]).T
#q_vel = np.mat([msg.velocity[i] for i in self.arm_inds]).T
q_vel = (q_pos - self.last_q_pos) / 0.01
q_effort = np.mat([msg.effort[i] for i in self.arm_inds]).T
fk_pos, fk_rot = self.kinematics.FK(q_pos)
J = self.kinematics.jacobian(q_pos)
F, res, rank, sing = np.linalg.lstsq(J.T, q_effort)
#print "Force:", F[0:3]
#print "Torque:", F[3:6]
self.publish_vector(fk_pos, 0.01 * F[0:3], 0)
self.publish_vector(fk_pos, 0.04 * F[3:6], 1)
H = self.kinematics.inertia(q_pos)
CI = self.kinematics.cart_inertia(q_pos)
#print q_vel
#print np.round(np.log10(np.fabs(CI)))
#print np.linalg.norm(H, 2), np.linalg.norm(CI, 2), np.linalg.cond(J * np.linalg.inv(H) * J.T), np.linalg.cond(H)
x_vel = J * q_vel
#print np.linalg.norm(x_vel[:3],2)
#print np.linalg.norm(x_vel, 2)
#print np.linalg.norm((CI - self.last_CI) / 0.01 * x_vel, 2)
print np.linalg.norm((CI * x_vel)[:3], 2)
#print np.around(CI * x_vel, 3)
self.last_CI = CI
self.last_q_pos = q_pos
def publish_vector(self, loc, v, m_id):
m = Marker()
m.header.frame_id = "torso_lift_link"
m.header.stamp = rospy.Time()
m.ns = "force_torque"
m.id = m_id
m.type = Marker.ARROW
m.action = Marker.ADD
m.points.append(Point(*loc))
m.points.append(Point(*(loc + v)))
m.scale = Vector3(0.01, 0.02, 0.01)
m.color = self.colors[m_id]
self.vis_pub.publish(m)
class JointForceViz:
def __init__(self, arm):
self.arm = arm
self.vis_pub = rospy.Publisher("force_torque_markers", Marker)
if arm == 'r':
self.arm_inds = JOINT_STATE_INDS_R
else:
self.arm_inds = JOINT_STATE_INDS_L
model = urdf.create_model_from_file(
"/etc/ros/diamondback/urdf/robot.xml")
tree = kdlp.tree_from_urdf_model(model)
chain = tree.getChain("torso_lift_link", "r_gripper_tool_frame")
link_dict = {0: 0, 1: 1, 2: 2, 3: 3, 4: 4, 5: 5, 6: 6, 7: 11}
self.kinematics = KDLArmKinematics(chain=chain, link_dict=link_dict)
self.colors = [ColorRGBA(1., 0., 0., 1.), ColorRGBA(0., 1., 0., 1.)]
rospy.Subscriber("joint_states", JointState, self.joint_states_cb)
self.last_CI = np.zeros((6, 6))
self.last_q_pos = np.zeros((7, 1))
def joint_states_cb(self, msg):
q_pos = np.mat([msg.position[i] for i in self.arm_inds]).T
#q_vel = np.mat([msg.velocity[i] for i in self.arm_inds]).T
q_vel = (q_pos - self.last_q_pos) / 0.01
q_effort = np.mat([msg.effort[i] for i in self.arm_inds]).T
fk_pos, fk_rot = self.kinematics.FK(q_pos)
J = self.kinematics.jacobian(q_pos)
F, res, rank, sing = np.linalg.lstsq(J.T, q_effort)
#print "Force:", F[0:3]
#print "Torque:", F[3:6]
self.publish_vector(fk_pos, 0.01 * F[0:3], 0)
self.publish_vector(fk_pos, 0.04 * F[3:6], 1)
H = self.kinematics.inertia(q_pos)
CI = self.kinematics.cart_inertia(q_pos)
#print q_vel
#print np.round(np.log10(np.fabs(CI)))
#print np.linalg.norm(H, 2), np.linalg.norm(CI, 2), np.linalg.cond(J * np.linalg.inv(H) * J.T), np.linalg.cond(H)
x_vel = J * q_vel
#print np.linalg.norm(x_vel[:3],2)
#print np.linalg.norm(x_vel, 2)
#print np.linalg.norm((CI - self.last_CI) / 0.01 * x_vel, 2)
print np.linalg.norm((CI * x_vel)[:3], 2)
#print np.around(CI * x_vel, 3)
self.last_CI = CI
self.last_q_pos = q_pos
def publish_vector(self, loc, v, m_id):
m = Marker()
m.header.frame_id = "torso_lift_link"
m.header.stamp = rospy.Time()
m.ns = "force_torque"
m.id = m_id
m.type = Marker.ARROW
m.action = Marker.ADD
m.points.append(Point(*loc))
m.points.append(Point(*(loc + v)))
m.scale = Vector3(0.01, 0.02, 0.01)
m.color = self.colors[m_id]
self.vis_pub.publish(m)
def __init__(self, arm):
self.arm = arm
self.vis_pub = rospy.Publisher("force_torque_markers", Marker)
if arm == 'r':
self.arm_inds = JOINT_STATE_INDS_R
else:
self.arm_inds = JOINT_STATE_INDS_L
model = urdf.create_model_from_file(
"/etc/ros/diamondback/urdf/robot.xml")
tree = kdlp.tree_from_urdf_model(model)
chain = tree.getChain("torso_lift_link", "r_gripper_tool_frame")
link_dict = {0: 0, 1: 1, 2: 2, 3: 3, 4: 4, 5: 5, 6: 6, 7: 11}
self.kinematics = KDLArmKinematics(chain=chain, link_dict=link_dict)
self.colors = [ColorRGBA(1., 0., 0., 1.), ColorRGBA(0., 1., 0., 1.)]
rospy.Subscriber("joint_states", JointState, self.joint_states_cb)
def __init__(self, arm):
self.arm = arm
self.vis_pub = rospy.Publisher("force_torque_markers", Marker)
if arm == 'r':
self.arm_inds = JOINT_STATE_INDS_R
else:
self.arm_inds = JOINT_STATE_INDS_L
model = urdf.create_model_from_file("/etc/ros/diamondback/urdf/robot.xml")
tree = kdlp.tree_from_urdf_model(model)
chain = tree.getChain("torso_lift_link", "r_gripper_tool_frame")
link_dict = {0:0, 1:1, 2:2, 3:3, 4:4, 5:5, 6:6, 7:11}
self.kinematics = KDLArmKinematics(chain=chain, link_dict=link_dict)
self.colors = [ColorRGBA(1., 0., 0., 1.), ColorRGBA(0., 1., 0., 1.)]
rospy.Subscriber("joint_states", JointState, self.joint_states_cb)
