def __init__(self):
self._pr2 = PR2JointMover(time_to_reach=1)
self._ik = KinematicsLoader(self._pr2.robot_state)
self._pr2box = PR2Box()
self._rate = rospy.Rate(10)
self._human_joints = None
rospy.Subscriber("human_joints", sensor_msgs.msg.JointState,
self.joints_cb)
class JointsMirror:
def __init__(self):
self._pr2 = PR2JointMover(time_to_reach=1)
self._ik = KinematicsLoader(self._pr2.robot_state)
self._pr2box = PR2Box()
self._rate = rospy.Rate(10)
self._human_joints = None
rospy.Subscriber("human_joints", sensor_msgs.msg.JointState,
self.joints_cb)
def joints_cb(self, data):
assert isinstance(data, sensor_msgs.msg.JointState)
self._human_joints = data.position
def act(self):
while not rospy.is_shutdown():
self._rate.sleep()
if self._human_joints is None:
rospy.logwarn("No human joints received, waiting...")
continue
#left arm
pr2_joints_left = [0] * 7
pr2_joints_left[0] = normalize(self._human_joints[2], -pi, pi)
pr2_joints_left[1] = normalize(-(self._human_joints[4] - pi/2), -pi, pi)
pr2_joints_left[2] = normalize(-(self._human_joints[6] - pi/2), -pi, pi)
pr2_joints_left[3] = normalize(self._human_joints[0], -pi, pi)
#right_arm
pr2_joints_right = [0] * 7
pr2_joints_right[0] = normalize(self._human_joints[3], -pi, pi)
pr2_joints_right[1] = normalize(-(self._human_joints[5] - pi/2), -pi, pi)
pr2_joints_right[2] = normalize((self._human_joints[7] - pi/2), -pi, pi)
pr2_joints_right[3] = normalize(self._human_joints[1], -pi, pi)
#get left fk pose
l_execute = True
l_pose = self._ik.get_left_arm_fk_pose(pr2_joints_left,
self._pr2box.frame_id)[0]
rospy.loginfo("testing pose: %s", l_pose)
if (l_pose[1] < 0.2):
rospy.logwarn("Left arm too close to center, not executing"
)
l_execute = False
if self._pr2box.point_inside(l_pose):
rospy.logwarn("Warning, desired pose: %s is dangerous for left arm,"
"not executing!", l_pose)
l_execute = False
if (l_execute):
self._pr2.set_arm_state(pr2_joints_left, "left", wait=False)
#get right ik pose
r_pose = self._ik.get_right_arm_fk_pose(pr2_joints_right,
self._pr2box.frame_id)[0]
r_execute = True
if (r_pose[1] > -0.2):
rospy.logwarn("Right arm too close to center, not executing"
)
r_execute = False
if (self._pr2box.point_inside(r_pose)):
rospy.logwarn("Warning, desired pose: %s is dangerous for right arm,"
"not executing!", r_pose)
r_execute = False
if (r_execute):
self._pr2.set_arm_state(pr2_joints_right, "right", wait=False)
