def getCartImpWristTraj(self, js, goal_T_B_W):
init_js = copy.deepcopy(js)
init_T_B_W = self.fk_solver.calculateFk("right_arm_7_link", init_js)
T_B_Wd = goal_T_B_W
T_B_W_diff = PyKDL.diff(init_T_B_W, T_B_Wd, 1.0)
steps = int(max(T_B_W_diff.vel.Norm() / 0.05, T_B_W_diff.rot.Norm() / (20.0 / 180.0 * math.pi)))
if steps < 2:
steps = 2
self.updateJointLimits(init_js)
self.updateMimicJoints(init_js)
q_list = []
for f in np.linspace(0.0, 1.0, steps):
T_B_Wi = PyKDL.addDelta(init_T_B_W, T_B_W_diff, f)
q_out = self.fk_solver.simulateTrajectory("right_arm_7_link", init_js, T_B_Wi)
if q_out == None:
# print "error: getCartImpWristTraj: ", str(f)
return None
q_list.append(q_out)
for i in range(7):
joint_name = self.fk_solver.ik_joint_state_name["right_arm_7_link"][i]
init_js[joint_name] = q_out[i]
return q_list
def handle_cart_cmd(self, msg):
side = self.side
try:
# Get the current position of the hydra
hydra_frame = fromTf(self.listener.lookupTransform(
'/hydra_base',
'/hydra_'+self.SIDE_STR[side]+'_grab',
rospy.Time(0)))
# Get the current position of the end-effector
tip_frame = fromTf(self.listener.lookupTransform(
'/world',
self.tip_link,
rospy.Time(0)))
# Capture the current position if we're starting to move
if not self.deadman_engaged:
self.deadman_engaged = True
self.cmd_origin = hydra_frame
self.tip_origin = tip_frame
else:
self.deadman_max = max(self.deadman_max, msg.axes[self.DEADMAN[side]])
# Update commanded TF frame
cmd_twist = kdl.diff(self.cmd_origin, hydra_frame)
cmd_twist.vel = self.scale*self.deadman_max*cmd_twist.vel
self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex:
rospy.logwarn(str(ex))
def handle_cart_cmd(self, scaling):
""""""
try:
# Get the current position of the hydra
input_frame = fromTf(self.listener.lookupTransform(
self.input_ref_frame_id,
self.input_frame_id,
rospy.Time(0)))
# Get the current position of the end-effector
tip_frame = fromTf(self.listener.lookupTransform(
self.input_ref_frame_id,
self.tip_link,
rospy.Time(0)))
# Capture the current position if we're starting to move
if not self.deadman_engaged:
self.deadman_engaged = True
self.cmd_origin = input_frame
self.tip_origin = tip_frame
else:
self.cart_scaling = scaling
# Update commanded TF frame
cmd_twist = kdl.diff(self.cmd_origin, input_frame)
cmd_twist.vel = self.scale*self.cart_scaling*cmd_twist.vel
#rospy.logwarn(cmd_twist)
self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex:
rospy.logwarn(str(ex))
def integ_error(self, twist_err, dT=0.01):
"""Apply timestep-wise error integration.
"""
T = PyKDL.addDelta(self.prev_frame, twist_err,
dT) # --> calculate the Integral
#T = PyKDL.addDelta(self.init_frame, twist_err, self.traj_elapse)
#return PyKDL.diff(self.init_frame, self.cur_frame, self.traj_elapse)
return PyKDL.diff(self.init_frame, T, self.traj_elapse)
def calculateWrenchesForTransportTask(self, ext_wrenches_W, transport_T_B_O):
ext_wrenches_O = []
for i in range(len(transport_T_B_O)-1):
diff_B_O = PyKDL.diff(transport_T_B_O[i], transport_T_B_O[i+1])
# simulate object motion and calculate expected wrenches
for t in np.linspace(0.0, 1.0, 5):
T_B_Osim = PyKDL.addDelta(transport_T_B_O[i], diff_B_O, t)
T_Osim_B = T_B_Osim.Inverse()
for ewr in ext_wrenches_W:
ext_wrenches_O.append(PyKDL.Frame(T_Osim_B.M) * ewr)
return ext_wrenches_O
def test_compute_twist(self):
for _ in range(100):
T0 = br.transform.random()
vel = np.random.rand(3)
rot = np.random.rand(3)
kdl_twist = PyKDL.Twist(PyKDL.Vector(*vel), PyKDL.Vector(*rot))
F0 = posemath.fromMatrix(T0)
F1 = PyKDL.addDelta(F0, kdl_twist)
T1 = posemath.toMatrix(F1)
twist = ru.transforms.compute_twist(F0, F1)
np.testing.assert_allclose(twist, np.hstack((vel, rot)))
def main():
print "\n\n==== To and from tf"
tf = ((1,2,3), (1,0,0,0))
print tf
print toTf(fromTf(tf))
print "\n\n==== To and from msg"
p = Pose()
p.orientation.x = 1
p.position.x = 4
print p
print toMsg(fromMsg(p))
print "\n\n==== To and from matrix"
m = numpy.array([[0,1,0,2], [0,0,1,3],[1,0,0,4],[0,0,0,1]])
print m
print toMatrix(fromMatrix(m))
print "\n\n==== Math"
print toTf( fromMsg(p) * fromMatrix(m))
print PyKDL.diff(fromMsg(p), fromMatrix(m))
print PyKDL.addDelta(fromMsg(p), PyKDL.diff(fromMatrix(m), fromMsg(p)), 2.0)
def move(self, desiredPose, maxForce):
currentPose = self.robot.get_desired_position()
currentPose.p = currentPose.p
forceArray = np.empty((0, 4), float)
displacements = np.array([0], float)
# Remove z rotation
angle = np.arccos(
PyKDL.dot(desiredPose.M.UnitX(), currentPose.M.UnitX()))
rot = PyKDL.Rotation.Rot(desiredPose.M.UnitZ(), angle)
# Added offset representing tooltip
desiredPosition = desiredPose.p - desiredPose.M.UnitZ(
) * self.toolOffset
desiredPoseWithOffset = PyKDL.Frame(desiredPose.M, desiredPosition)
measuredPose_previous = self.robot.get_current_position()
startForce = self.force[1]
while not rospy.is_shutdown():
# get current and desired robot pose (desired is the top of queue)
# compute the desired twist "x_dot" from motion command
xDotMotion = resolvedRates(
self.resolvedRatesConfig, currentPose,
desiredPoseWithOffset) # xDotMotion is type [PyKDL.Twist]
currentPose = PyKDL.addDelta(currentPose, xDotMotion,
self.resolvedRatesConfig['dt'])
#Save displacement and force in different arrays
if type(self.force) == type(None):
rospy.logwarn(
"Probe Service: No forces detected. Not moving and returning None"
)
return None, None
data = np.array([self.force])
if xDotMotion.vel.Norm() <= 0.001 and xDotMotion.rot.Norm() <= 0.1:
break
if self.force[1] - startForce > maxForce:
rospy.logwarn("Max Force Exceeded: " + str(self.force))
break
self.robot.move(currentPose, interpolate=False)
self.rate.sleep()
measuredPose_current = self.robot.get_current_position()
currentDisplacement = measuredPose_current.p - measuredPose_previous.p
# currentDisplacement = xDotMotion.vel.Norm() * self.resolvedRatesConfig['dt']
currentDisplacement = PyKDL.dot(currentDisplacement,
desiredPose.M.UnitZ())
# currentDisplacement = displacements[len(displacements)-1] + currentDisplacement
forceArray = np.append(forceArray, data, axis=0)
displacements = np.append(displacements, [currentDisplacement])
return displacements.tolist(), forceArray.tolist()
def replan_movement(self,
goal_kdl_frame,
current_kdl_frame,
interp_time=2):
#Check if really new goal:
if not self.check_if_new_goal(goal_kdl_frame):
rospy.logwarn("Not a new goal")
return
#If we got here, the message is really different
self.new_goal(goal_kdl_frame)
self.set_current_pose(current_kdl_frame)
#find distance
rot_dist = kdl.diff(self.current_pose.M, self.dst_pose.M)
if rot_dist[0] != rot_dist[0]:
#getting nans here, work around bug by rotating current pose a little
self.current_pose.M.DoRotZ(2 * 3.141509 / 180.0)
dist = kdl.diff(kdl.Frame(self.current_pose), kdl.Frame(self.dst_pose))
#print("current pose:")
#print self.current_pose.M.GetRPY()
dt = 0.05 #looks smoother at 20Hz
total_time = interp_time #sec
times = int(total_time / dt)
dx = 1.0 / times
self.start_time = rospy.Time.now()
self.pose_vector = []
for i in range(times + 1):
if i == 0:
inter_pose = self.current_pose
else:
inter_pose = kdl.addDelta(kdl.Frame(self.current_pose), dist,
dx * i)
inter_time = self.start_time + rospy.Duration(dt) * i
#rospy.logwarn("Generated pose:")
#print inter_pose
#print inter_pose.p
#print inter_pose.M.GetRPY()
#rospy.logwarn("Original pose:")
#print self.current_pose
self.pose_vector.append([inter_time, kdl.Frame(inter_pose)])
def derive_features(frame, feature_function, dd=0.01):
''' numerically derive the task angle function around the given frame.
Also computes the inverse: this yields the instantaneous rotation axes
for each angle.
'''
q0 = numpy.mat(feature_function(frame))
size = q0.size
print q0.size
jac_i = numpy.mat(numpy.zeros((q0.size, 6)))
for i in range(6):
twist = kdl.Twist()
twist[i] = 1.0
frame_moved = kdl.addDelta(frame, twist, dd)
q_i = numpy.mat( feature_function(frame_moved) )
jac_i[0:size,i] = ((q_i - q0) / dd).T
# the columns of jac are six twists to be displayed
jac = numpy.linalg.pinv(jac_i)
return (jac_i, jac)
def move(self, desiredPose, maxForce):
currentPose = self.robot.get_desired_position()
currentPose.p = currentPose.p
displacements = np.array([0], float)
# Remove z rotation
angle = np.arccos(
PyKDL.dot(desiredPose.M.UnitX(), currentPose.M.UnitX()))
rot = PyKDL.Rotation.Rot(desiredPose.M.UnitZ(), angle)
# Added offset representing tooltip
desiredPosition = desiredPose.p - desiredPose.M.UnitZ(
) * self.toolOffset
desiredPoseWithOffset = PyKDL.Frame(desiredPose.M, desiredPosition)
measuredPose_previous = self.robot.get_current_position()
startForce = self.force[1]
while not rospy.is_shutdown():
# get current and desired robot pose (desired is the top of queue)
# compute the desired twist "x_dot" from motion command
xDotMotion = resolvedRates(
self.resolvedRatesConfig, currentPose,
desiredPoseWithOffset) # xDotMotion is type [PyKDL.Twist]
currentPose = PyKDL.addDelta(currentPose, xDotMotion,
self.resolvedRatesConfig['dt'])
if xDotMotion.vel.Norm() <= 0.001 and xDotMotion.rot.Norm() <= 0.1:
break
self.robot.move(currentPose, interpolate=False)
self.rate.sleep()
measuredPose_current = self.robot.get_current_position()
currentDisplacement = measuredPose_current.p - measuredPose_previous.p
currentDisplacement = PyKDL.dot(currentDisplacement,
desiredPose.M.UnitZ())
displacements = np.append(displacements, [currentDisplacement])
return displacements.tolist()
def compute_increment(goal_features, feature_set, frame, dd=0.1): tw = compute_direction(goal_features, feature_set, frame) print 'twist: '+ str(tw) return kdl.addDelta(frame, tw, dd)
def pose_oplus(kdl_pose, step):
t = PyKDL.Twist()
for i in range(pose_width):
t[i] = step[i,0]
next_pose = kdl_pose * PyKDL.addDelta(PyKDL.Frame(), t, 1.0)
return next_pose
def find_checkerboard_service(self, req):
poses = []
min_x = self.ros_image.width
min_y = self.ros_image.height
max_x = 0
max_y = 0
for i in range(10):
rospy.sleep(0.5)
#copy the image over
with self.mutex:
image = self.ros_image
result = self.find_checkerboard_pose(image, req.corners_x,
req.corners_y, req.spacing_x,
req.spacing_y,
self.width_scaling,
self.height_scaling)
if result is None:
rospy.logerr("Couldn't find a checkerboard")
continue
p, corners = result
poses.append(p)
for j in range(corners.cols):
x = cv.GetReal2D(corners, 0, j)
y = cv.GetReal2D(corners, 1, j)
min_x = min(min_x, x)
max_x = max(max_x, x)
min_y = min(min_y, y)
max_y = max(max_y, y)
# convert all poses to twists
twists = []
for p in poses:
twists.append(
PyKDL.diff(PyKDL.Frame.Identity(), posemath.fromMsg(p.pose)))
# get average twist
twist_res = PyKDL.Twist()
PyKDL.SetToZero(twist_res)
for t in twists:
for i in range(3):
twist_res.vel[i] += t.vel[i] / len(poses)
twist_res.rot[i] += t.rot[i] / len(poses)
# get noise
noise_rot = 0
noise_vel = 0
for t in twists:
n_vel = (t.vel - twist_res.vel).Norm()
n_rot = (t.rot - twist_res.rot).Norm()
if n_vel > noise_vel:
noise_vel = n_vel
if n_rot > noise_rot:
noise_rot = n_rot
# set service resul
pose_res = PyKDL.addDelta(PyKDL.Frame.Identity(), twist_res, 1.0)
pose_msg = PoseStamped()
pose_msg.header = poses[0].header
pose_msg.pose = posemath.toMsg(pose_res)
return GetCheckerboardPoseResponse(pose_msg, min_x, max_x, min_y,
max_y, noise_vel, noise_rot)
def my_adddelta(frame, twist, dt):
return (
kdlframe_to_narray(
PyKDL.addDelta(
narray_to_kdlframe(frame), narray_to_kdltwist(twist), dt)))
def find_checkerboard_service(self, req):
poses = []
min_x = self.ros_image.width
min_y = self.ros_image.height
max_x = 0
max_y = 0
for i in range(10):
rospy.sleep(0.5)
#copy the image over
with self.mutex:
image = self.ros_image
result = self.find_checkerboard_pose(image, req.corners_x,
req.corners_y, req.spacing_x,
req.spacing_y, self.width_scaling,
self.height_scaling)
if result is None:
rospy.logerr("Couldn't find a checkerboard")
continue
p, corners = result
poses.append(p)
for j in range(corners.cols):
x = cv.GetReal2D(corners, 0, j)
y = cv.GetReal2D(corners, 1, j)
min_x = min(min_x, x)
max_x = max(max_x, x)
min_y = min(min_y, y)
max_y = max(max_y, y)
# convert all poses to twists
twists = []
for p in poses:
twists.append(PyKDL.diff(PyKDL.Frame.Identity(), posemath.fromMsg(p.pose)))
# get average twist
twist_res = PyKDL.Twist()
PyKDL.SetToZero(twist_res)
for t in twists:
for i in range(3):
twist_res.vel[i] += t.vel[i]/len(poses)
twist_res.rot[i] += t.rot[i]/len(poses)
# get noise
noise_rot = 0
noise_vel = 0
for t in twists:
n_vel = (t.vel - twist_res.vel).Norm()
n_rot = (t.rot - twist_res.rot).Norm()
if n_vel > noise_vel:
noise_vel = n_vel
if n_rot > noise_rot:
noise_rot = n_rot
# set service resul
pose_res = PyKDL.addDelta(PyKDL.Frame.Identity(), twist_res, 1.0)
pose_msg = PoseStamped()
pose_msg.header = poses[0].header
pose_msg.pose = posemath.toMsg(pose_res)
return GetCheckerboardPoseResponse(pose_msg, min_x, max_x, min_y, max_y, noise_vel, noise_rot)
def integ_error(self, twist_err, dT):
"""Apply timestep-wise error integration.
"""
self.integ_frame = PyKDL.addDelta(self.integ_frame, twist_err, dT)
return PyKDL.diff(self.init_frame, self.integ_frame, self.traj_elapse)
def pose_oplus(kdl_pose, step):
t = PyKDL.Twist()
for i in range(pose_width):
t[i] = step[i, 0]
next_pose = kdl_pose * PyKDL.addDelta(PyKDL.Frame(), t, 1.0)
return next_pose
def run(self):
commandedPose = self.robot.get_desired_position() # find start pose
desiredPose = commandedPose # match original desired position at start
tic = time()
pauseTime = 30
while not rospy.is_shutdown():
toc = (time() - tic)
if np.floor(toc / pauseTime) == 1:
# ipdb.set_trace()
tic = time()
# Publish trajectory length at all times
self.trajStatusPub.publish(self.trajIndex)
# Check if there are any trajectories
try:
desiredPose = self.trajectory[0]
except IndexError:
# If no trajectory don't move, exit
continue
# get current and desired robot pose (desired is the top of queue)
currentPose = self.robot.get_current_position(
) # this is used to capture the error
# Update the wrist orientation so that the desired orientation matches the environment
if self.resolvedRatesConfig['b_wristOrient'] and self.inContact:
zcur = PyKDL.Vector(currentPose.M[0, 2], currentPose.M[1, 2],
currentPose.M[2, 2])
zdes = -PyKDL.Vector(self.getAverageForce())
zdes = self.coneLimit(zdes, self.resolvedRatesConfig['zref'],
np.pi / 2)
zdes.Normalize()
normalDirection = self.movingDirection * PyKDL.Vector(
self.forceProfile['defaultDir'][0],
self.forceProfile['defaultDir'][1],
self.forceProfile['defaultDir'][2])
zdes = self.projectNullSpace2(normalDirection, zdes)
axis = zcur * zdes
crossNorm = axis.Normalize()
dotResult = PyKDL.dot(zcur, zdes)
angle = math.atan2(crossNorm, dotResult)
diffTwist = PyKDL.Twist(PyKDL.Vector(), axis * angle)
desiredPose = PyKDL.addDelta(desiredPose, diffTwist, 1)
pass
# compute the desired twist "x_dot" from motion command [PyKDL.Twist]
[xDotMotion, goalPoseReached] = self.resolvedRates(currentPose, \
desiredPose)
# Do hybrid force motion if using the force controller
if self.forceProfile['b_forceOn']:
# compute the desired twist "x_dot" from force command [PyKDL.Twist]
#TRY NOT DOING THIS!
if self.resolvedRatesConfig['b_wristOrient'] and self.inContact:
try:
forceCtrlDir = zdes
except NameError:
forceCntrlDir = PyKDL.Vector(\
self.forceProfile['defaultDir'][0],
self.forceProfile['defaultDir'][1],
self.forceProfile['defaultDir'][2])
else:
forceCtrlDir = self.updateForceControlDir()
xDotForce = self.forceAdmittanceControl(forceCtrlDir)
[xDot, goalPoseReached
] = self.hybridPosForce(xDotMotion, xDotForce, forceCtrlDir)
else:
xDot = xDotMotion
# Check whether we have reached our goal
if goalPoseReached:
# ipdb.set_trace()
# When first reached the first goal pose, turn on the force controller
self.forceProfile['b_forceOn'] = True
if len(self.trajectory) > 1 and self.inContact:
# self.movingDirection=PyKDL.Vector((PyKDL.diff(self.trajectory[1],self.trajectory[0])).vel)
# self.movingDirection.Normalize()
self.trajectory.popleft()
self.trajIndex = self.trajIndex + 1
print("Trajectories finished: " + str(self.trajIndex))
pass
# else:
# ipdb.set_trace()
# apply the desired twist on the currnet pose
dt = self.resolvedRatesConfig['dt']
commandedPose = PyKDL.addDelta(commandedPose, xDot, dt)
#ipdb.set_trace()
# Move the robot
self.robot._arm__move_frame(commandedPose, interpolate=False)
self.rate.sleep()
from geometry_msgs.msg import Twist
rospy.init_node("moveable_frame")
frame = kdl.Frame()
twist = kdl.Twist()
def callback(msg):
global twist
lin = kdl.Vector(msg.linear.x, msg.linear.y, msg.linear.z)
rot = kdl.Vector(msg.angular.x, msg.angular.y, msg.angular.z)
twist = kdl.Twist(lin, rot)
frame_name = rospy.get_param("~frame_name", "/frame")
parent_name = rospy.get_param("~parent_name", "/base_link")
f = rospy.get_param("~rate", 30)
rospy.Subscriber("twist", Twist, callback)
tf_pub = tf.TransformBroadcaster()
rate = rospy.Rate(f)
while not rospy.is_shutdown():
t = kdl.Twist(twist)
# frame = kdl.addDelta(frame, t, 1.0/f) # global mode
frame = kdl.addDelta(frame, kdl.Frame(frame.M) * t, 1.0 / f) # local mode
tf_pub.sendTransform(frame.p, frame.M.GetQuaternion(), rospy.Time.now(), frame_name, parent_name)
rate.sleep()
def execute_cb(self, goal):
# helper variables
success = True
if self.robot_state.joint_impedance_active:
self._result.error_code = 0
self._as.set_succeeded(self._result)
print "MoveCartesianTrajectory ", self._action_name, " points: ", len(goal.trajectory.points)
init_T_B_W = self.robot_state.fk_solver.calculateFk(self.wrist_name, self.robot_state.getJsPos())
init_T_B_T = init_T_B_W * self.robot_state.T_W_T[self.wrist_name]
current_dest_point_idx = 0
while True:
if self._as.is_preempt_requested():
rospy.loginfo("%s: Preempted" % self._action_name)
self._as.set_preempted()
return
time_now = rospy.Time.now()
time_from_start = (time_now - goal.trajectory.header.stamp).to_sec()
if time_from_start <= 0:
rospy.sleep(0.01)
continue
while time_from_start > goal.trajectory.points[current_dest_point_idx].time_from_start.to_sec():
current_dest_point_idx += 1
if current_dest_point_idx >= len(goal.trajectory.points):
break
if current_dest_point_idx >= len(goal.trajectory.points):
break
dest_time = goal.trajectory.points[current_dest_point_idx].time_from_start.to_sec()
dest_T_B_T = pm.fromMsg(goal.trajectory.points[current_dest_point_idx].pose)
if current_dest_point_idx > 0:
prev_time = goal.trajectory.points[current_dest_point_idx - 1].time_from_start.to_sec()
prev_T_B_T = pm.fromMsg(goal.trajectory.points[current_dest_point_idx - 1].pose)
else:
prev_time = 0.0
prev_T_B_T = init_T_B_T
f = (time_from_start - prev_time) / (dest_time - prev_time)
if f < 0 or f > 1:
print "error: MoveCartesianTrajectory f: ", str(f)
diff_T_B_T = PyKDL.diff(prev_T_B_T, dest_T_B_T, 1.0)
T_B_Ti = PyKDL.addDelta(prev_T_B_T, diff_T_B_T, f)
T_B_Wi = T_B_Ti * self.robot_state.T_W_T[self.wrist_name].Inverse()
q_out = self.robot_state.fk_solver.simulateTrajectory(self.wrist_name, self.robot_state.getJsPos(), T_B_Wi)
if q_out == None:
self._result.error_code = -3 # PATH_TOLERANCE_VIOLATED
self._as.set_aborted(self._result)
return
for i in range(7):
joint_name = self.robot_state.fk_solver.ik_joint_state_name[self.wrist_name][i]
self.robot_state.js.position[self.robot_state.joint_name_idx_map[joint_name]] = q_out[i]
self.robot_state.arm_cmd[self.wrist_name] = T_B_Ti
# publish the feedback
self._feedback.header.stamp = time_now
self._feedback.desired = pm.toMsg(T_B_Ti)
self._feedback.actual = pm.toMsg(T_B_Ti)
self._feedback.error = pm.toMsg(PyKDL.Frame())
self._as.publish_feedback(self._feedback)
rospy.sleep(0.01)
self._result.error_code = 0
self._as.set_succeeded(self._result)
