def smoothPlan(self, transforms):
"""
Smooth plan function which solves for all IK solutions for each transform given.
It then chooses the joint DOF values which are closest to current DOF values.
List of transforms along path -> transforms
"""
if len(transforms) < 1:
rospy.ERROR('Not enough transforms!')
if len(transforms) == 1:
initTransform = self.arm.manip.GetEndEffectorTransform()
transforms = [initTransform, transforms[0]]
trajectory = []
armIndices = self.arm.manip.GetArmIndices()
currJoints = self.arm.manip.GetRobot().GetDOFValues(armIndices)
# currJoints = self.arm.get_joint_positions()
for transform in transforms:
allJoints = self.arm.manip.FindIKSolutions(transform, self.filter_options)
if not allJoints.size:
rospy.logwarn('IK Failed on ' + self.rl_long + 'arm.')
return None
allJoints = [ku.closer_joint_angles(joints, currJoints) for joints in allJoints]
normDifferences = [norm(currJoints-joints,2) for joints in allJoints]
minIndex = normDifferences.index(min(normDifferences))
trajectory.append(allJoints[minIndex])
return trajectory
def cart_to_joint(manip, matrix4, ref_frame, targ_frame, filter_options = 0):
robot = manip.GetRobot()
worldFromEE = transform_relative_pose_for_ik(manip, matrix4, ref_frame, targ_frame)
joint_positions = manip.FindIKSolution(worldFromEE, filter_options)
if joint_positions is None: return joint_positions
current_joints = robot.GetDOFValues(manip.GetArmIndices())
joint_positions_unrolled = ku.closer_joint_angles(joint_positions, current_joints)
return joint_positions_unrolled
def set_joints(self, joints):
j = self.robot.GetDOFValues()
rarm_curr = j[self.rarm_indices]
rarm_goal = joints[0]
rarm_goal = ku.closer_joint_angles(rarm_goal, rarm_curr)
larm_curr = j[self.larm_indices]
larm_goal = joints[1]
larm_goal = ku.closer_joint_angles(larm_goal, larm_curr)
j[self.larm_indices] = larm_goal
j[self.lgripper_indices] = joints[3]
j[self.rarm_indices] = rarm_goal
j[self.rgripper_indices] = joints[2]
with self.env:
self.robot.SetJointValues(j)
def set_joints(self, joints):
j = self.robot.GetDOFValues()
rarm_curr = j[self.rarm_indices]
rarm_goal = joints[0]
rarm_goal = ku.closer_joint_angles(rarm_goal, rarm_curr)
larm_curr = j[self.larm_indices]
larm_goal = joints[1]
larm_goal = ku.closer_joint_angles(larm_goal, larm_curr)
j[self.larm_indices] = larm_goal
j[self.lgripper_indices] = joints[3]
j[self.rarm_indices] = rarm_goal
j[self.rgripper_indices] = joints[2]
with self.env:
self.robot.SetJointValues(j)
def callback(self,msg):
self.counter+= 1
if self.counter % 20 != 0:
return
if self.last_msg is None:
self.last_msg = msg
return
for i in xrange(2):
paddle = msg.paddles[i]
if (paddle.trigger>.5) and not (self.last_msg.paddles[i].trigger>.5):
xcur, ycur, zcur = self.arms[i].get_pose_matrix("torso_lift_link", "%s_gripper_tool_frame"%self.arms[i].lr)[:3,3]
xcmd, ycmd, zcmd = (paddle.transform.translation.x * self.scale,
paddle.transform.translation.y * self.scale,
paddle.transform.translation.z * self.scale)
self.xoffset, self.yoffset, self.zoffset = xcur-xcmd, ycur-ycmd, zcur-zcmd
print "engaging %s arm"%self.arms[i].lr
elif (paddle.trigger>.5) and (self.last_msg.paddles[i].trigger > .5):
x = paddle.transform.translation.x * self.scale + self.xoffset
y = paddle.transform.translation.y * self.scale + self.yoffset
z = paddle.transform.translation.z * self.scale + self.zoffset
xx = paddle.transform.rotation.x
yy = paddle.transform.rotation.y
zz = paddle.transform.rotation.z
ww = paddle.transform.rotation.w
joint_msg = JointCommand()
mat4 = conv.trans_rot_to_hmat([x,y,z], [xx,yy,zz,ww])
#print mat4
cur_joints = self.arms[i].get_joint_positions()
ik_joints = self.arms[i].cart_to_joint(mat4, "torso_lift_link", "r_gripper_tool_frame",filter_options=18)
self.arms[i].set_cart_target([xx,yy,zz,ww], [x,y,z], "torso_lift_link")
if ik_joints is not None:
ik_joints_unrolled = kinematics_utils.closer_joint_angles(ik_joints, cur_joints)
joint_msg.joints = ik_joints_unrolled
self.joint_pub.publish(joint_msg)
print "ik succeeded"
else:
print "ik failed"
#mat = transformations.quaternion_matrix([xx,yy,zz,ww])
#mat[:3,3] = [x,y,z]
#try: self.arms[i].goto_pose_matrix(mat, "torso_lift_link", self.arms[i].tool_frame);
#except IKFail: print "%s ik failed"%self.arms[i].lrlong
#self.grippers[i].set_angle_goal(.08*(1-paddle.trigger))
if paddle.buttons[1]: self.grippers[i].set_angle_goal(0)
if paddle.buttons[2]: self.grippers[i].set_angle_goal(.08)
if (msg.paddles[1].joy[0] > 0) or (msg.paddles[1].joy[1] > 0):
if msg.paddles[0].buttons[START]:
pan,tilt = self.pr2.head.get_joint_positions()
pan += .1*msg.paddles[1].joy[0]
tilt += .15*msg.paddles[1].joy[1]
self.pr2.head.set_pan_tilt(pan,tilt)
else:
self.pr2.base.set_twist((.1*msg.paddles[1].joy[1], -.1*msg.paddles[1].joy[0], msg.paddles[0].joy[0]))
self.last_msg = msg
def goto_joint_positions(self, positions_goal):
positions_cur = self.get_joint_positions()
positions_goal = ku.closer_joint_angles(positions_goal, positions_cur)
TrajectoryControllerWrapper.goto_joint_positions(self, positions_goal)