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 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 updateCom(T_B_O, T_B_O_2, contacts_O, com_pt, com_weights, m_id=0, pub_marker=None):
diff_B = PyKDL.diff(T_B_O, T_B_O_2)
up_v_B = PyKDL.Vector(0, 0, 1)
n_v_B = diff_B.rot * up_v_B
if pub_marker != None:
m_id = pub_marker.publishVectorMarker(
PyKDL.Vector(), diff_B.rot, m_id, r=0, g=1, b=0, frame="world", namespace="default", scale=0.01
)
m_id = pub_marker.publishVectorMarker(
PyKDL.Vector(), n_v_B, m_id, r=1, g=1, b=1, frame="world", namespace="default", scale=0.01
)
n_O = PyKDL.Frame(T_B_O.Inverse().M) * n_v_B
n_O_2 = PyKDL.Frame(T_B_O_2.Inverse().M) * n_v_B
# get all com points that lies in the positive direction from the most negative contact point with respect to n_v in T_B_O and T_B_O_2
c_dot_list = []
c_dot_list_2 = []
for c in contacts_O:
dot = PyKDL.dot(c, n_O)
c_dot_list.append(dot)
dot_2 = PyKDL.dot(c, n_O_2)
c_dot_list_2.append(dot_2)
# get all com points that lies in the positive direction from given contact
for contact_idx in range(0, len(c_dot_list)):
for com_idx in range(0, len(com_pt)):
c = com_pt[com_idx]
dot = PyKDL.dot(c, n_O)
dot_2 = PyKDL.dot(c, n_O_2)
if dot > c_dot_list[contact_idx] and dot_2 > c_dot_list_2[contact_idx]:
com_weights[com_idx] += 1
return m_id
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 updateMouthFrames(self, head_frame):
mouth_frame = copy.deepcopy(head_frame)
## Position
mouth_frame.p[2] -= self.head_z_offset
## Rotation
rot = mouth_frame.M
## head_z = np.array([rot.UnitX()[0], rot.UnitX()[1], rot.UnitX()[2]])
tx = PyKDL.Vector(1.0, 0.0, 0.0)
ty = PyKDL.Vector(0.0, 1.0, 0.0)
# Projection to xy plane
px = PyKDL.dot(tx, rot.UnitZ())
py = PyKDL.dot(ty, rot.UnitZ())
mouth_y = rot.UnitY()
mouth_z = PyKDL.Vector(px, py, 0.0)
mouth_z.Normalize()
mouth_x = mouth_y * mouth_z
mouth_y = mouth_z * mouth_x
mouth_rot = PyKDL.Rotation(mouth_x, mouth_y, mouth_z)
mouth_frame.M = mouth_rot
mouth_frame_off = head_frame.Inverse()*mouth_frame
if self.mouth_frame_off == None:
self.mouth_frame_off = mouth_frame_off
else:
self.mouth_frame_off.p = (self.mouth_frame_off.p + mouth_frame_off.p)/2.0
pre_quat = geometry_msgs.msg.Quaternion()
pre_quat.x = self.mouth_frame_off.M.GetQuaternion()[0]
pre_quat.y = self.mouth_frame_off.M.GetQuaternion()[1]
pre_quat.z = self.mouth_frame_off.M.GetQuaternion()[2]
pre_quat.w = self.mouth_frame_off.M.GetQuaternion()[3]
cur_quat = geometry_msgs.msg.Quaternion()
cur_quat.x = mouth_frame_off.M.GetQuaternion()[0]
cur_quat.y = mouth_frame_off.M.GetQuaternion()[1]
cur_quat.z = mouth_frame_off.M.GetQuaternion()[2]
cur_quat.w = mouth_frame_off.M.GetQuaternion()[3]
# check close quaternion and inverse
if np.dot(self.mouth_frame_off.M.GetQuaternion(), mouth_frame_off.M.GetQuaternion()) < 0.0:
cur_quat.x *= -1.
cur_quat.y *= -1.
cur_quat.z *= -1.
cur_quat.w *= -1.
quat = qt.slerp(pre_quat, cur_quat, 0.5)
self.mouth_frame_off.M = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
def updateBowlcenFrames(self, bowl_frame):
bowl_cen_frame = copy.deepcopy(bowl_frame)
## Rotation
rot = bowl_cen_frame.M
## bowl_z = np.array([rot.UnitX()[0], rot.UnitX()[1], rot.UnitX()[2]])
tx = PyKDL.Vector(1.0, 0.0, 0.0)
ty = PyKDL.Vector(0.0, 1.0, 0.0)
# Projection to xy plane
px = PyKDL.dot(tx, rot.UnitZ())
py = PyKDL.dot(ty, rot.UnitZ())
bowl_cen_y = rot.UnitY()
bowl_cen_z = PyKDL.Vector(px, py, 0.0)
bowl_cen_z.Normalize()
bowl_cen_x = bowl_cen_y * bowl_cen_z
bowl_cen_y = bowl_cen_z * bowl_cen_x
bowl_cen_rot = PyKDL.Rotation(bowl_cen_x, bowl_cen_y, bowl_cen_z)
bowl_cen_frame.M = bowl_cen_rot
## Position
bowl_cen_frame.p[2] -= self.bowl_z_offset
bowl_cen_frame.p += bowl_cen_z * self.bowl_forward_offset
if bowl_cen_y[2] > bowl_cen_x[2]:
# -90 x axis rotation
bowl_cen_frame = bowl_cen_frame * self.x_neg90_frame
else:
# 90 y axis rotation
bowl_cen_frame = bowl_cen_frame * self.y_90_frame
bowl_cen_frame_off = bowl_frame.Inverse()*bowl_cen_frame
if self.bowl_cen_frame_off == None:
self.bowl_cen_frame_off = bowl_cen_frame_off
else:
self.bowl_cen_frame_off.p = (self.bowl_cen_frame_off.p + bowl_cen_frame_off.p)/2.0
pre_quat = geometry_msgs.msg.Quaternion()
pre_quat.x = self.bowl_cen_frame_off.M.GetQuaternion()[0]
pre_quat.y = self.bowl_cen_frame_off.M.GetQuaternion()[1]
pre_quat.z = self.bowl_cen_frame_off.M.GetQuaternion()[2]
pre_quat.w = self.bowl_cen_frame_off.M.GetQuaternion()[3]
cur_quat = geometry_msgs.msg.Quaternion()
cur_quat.x = bowl_cen_frame_off.M.GetQuaternion()[0]
cur_quat.y = bowl_cen_frame_off.M.GetQuaternion()[1]
cur_quat.z = bowl_cen_frame_off.M.GetQuaternion()[2]
cur_quat.w = bowl_cen_frame_off.M.GetQuaternion()[3]
quat = qt.slerp(pre_quat, cur_quat, 0.5)
self.bowl_cen_frame_off.M = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
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 getGraspingAxis(bin_frame, obj_frame, object_name, simtrackUsed):
'''
this function assumes everything is represented in the quaternions in the /base_link frame
'''
if object_name.endswith('_scan'):
object_name = object_name[:-5]
dictObj = objDict()
objSpec = dictObj.getEntry(object_name)
F_bin_frame = posemath.fromTf(bin_frame)
F_obj_frame = posemath.fromTf(obj_frame)
objRed = F_obj_frame.M * kdl.Vector(1.0, 0.0, 0.0)
objGreen = F_obj_frame.M * kdl.Vector(0.0, 1.0, 0.0)
objBlue = F_obj_frame.M * kdl.Vector(0.0, 0.0, 1.0)
binRed = F_bin_frame.M * kdl.Vector(1.0, 0.0, 0.0)
binGreen = F_bin_frame.M * kdl.Vector(0.0, 1.0, 0.0)
binBlue = F_bin_frame.M * kdl.Vector(0.0, 0.0, 1.0)
rRProj = kdl.dot(objRed , binRed)
gRProj = kdl.dot(objGreen, binRed)
bRProj = kdl.dot(objBlue, binRed)
tmpApproach1 = [abs(rRProj), abs(gRProj), abs(bRProj)]
if simtrackUsed:
for i in range(3):
if i in objSpec.invalidApproachAxis:
tmpApproach1[i] = 0
tmpApproach2 = [rRProj, gRProj, bRProj]
axisApproach = tmpApproach1.index(max(tmpApproach1))
objAxes = [objRed, objGreen, objBlue]
tmpGrasp1 = []
for i in range(3):
if simtrackUsed:
if i == axisApproach or i in objSpec.invalidGraspAxis:
tmpGrasp1.append(0)
continue
tmpGrasp1.append(kdl.dot(objAxes[i], binBlue))
tmpGrasp2 = [abs(t) for t in tmpGrasp1]
axisGrasp = tmpGrasp2.index(max(tmpGrasp2))
return axisApproach, tmpApproach2[axisApproach]/abs(tmpApproach2[axisApproach]), axisGrasp, tmpGrasp1[axisGrasp]/abs(tmpGrasp1[axisGrasp])
def distanceLinePoint(self, line, pt):
a, b = line
v = b - a
ta = PyKDL.dot(v, a)
tb = PyKDL.dot(v, b)
tpt = PyKDL.dot(v, pt)
if tpt <= ta:
return (a-pt).Norm(), a, pt
elif tpt >= tb:
return (b-pt).Norm(), b, pt
else:
n = PyKDL.Vector(v.y(), -v.x(), v.z())
n.Normalize()
diff = PyKDL.dot(n, a) - PyKDL.dot(n, pt)
return abs(diff), pt + (diff * n), pt
def calc_R(xa, ya):
ret = []
""" calculate the minimum distance of each contact point from jar surface pt """
n = PyKDL.Frame(PyKDL.Rotation.RotX(xa)) * PyKDL.Frame(PyKDL.Rotation.RotY(ya)) * PyKDL.Vector(0, 0, 1)
for p in points:
ret.append(PyKDL.dot(n, p))
return numpy.array(ret)
def _update(self):
# Leave if ROS is not running or command is not valid
if rospy.is_shutdown() or self._last_goal is None:
return
# Calculate the goal pose
goal = self._get_goal()
# End-effector's pose
ee_pose = self._arm_interface.get_ee_pose_as_frame()
# End-effector's velocity
ee_vel = self._arm_interface.get_ee_vel_as_kdl_twist()
# Calculate pose error
error = PyKDL.diff(goal, ee_pose)
# End-effector wrench to achieve target
wrench = np.matrix(np.zeros(6)).T
for i in range(len(wrench)):
wrench[i] = -(self._Kp[i] * error[i] + self._Kd[i] * ee_vel[i])
# Compute jacobian transpose
JT = self._arm_interface.jacobian_transpose()
# Calculate the torques for the joints
tau = JT * wrench
# Store current pose target
self._last_goal = goal
self.publish_goal()
self.publish_joint_efforts(tau)
def _standing_still_for_x_seconds(self, timeout):
"""Check whether the robot is standing still for X seconds
:param timeout how many seconds must the robot be standing still before returning True
:type timeout float
:returns bool indicating whether the robot has been standing still for longer than timeout seconds"""
current_frame = self._robot.base.get_location().frame
now = rospy.Time.now()
if not self._last_pose_stamped:
self._last_pose_stamped = current_frame
self._last_pose_stamped_time = now
else:
current_yaw = current_frame.M.GetRPY()[2] # Get the Yaw
last_yaw = self._last_pose_stamped.M.GetRPY()[2] # Get the Yaw
# Compare the pose with the last pose and update if difference is larger than x
if kdl.diff(current_frame.p, self._last_pose_stamped.p).Norm() > 0.05 or abs(current_yaw - last_yaw) > 0.3:
# Update the last pose
self._last_pose_stamped = current_frame
self._last_pose_stamped_time = rospy.Time.now()
else:
print "Robot dit not move for x seconds: %f"%(now - self._last_pose_stamped_time).to_sec()
# Check whether we passed the timeout
if (now - self._last_pose_stamped_time).to_sec() > timeout:
return True
return False
def calc_R(px, py, pz):
R_7_M = PyKDL.Frame(rot_mx, PyKDL.Vector(px, py, pz))
ret = []
for idx in range(0,len(T_7bp_7i)):
diff = PyKDL.diff( T_7bp_7i[idx] * R_7_M, R_7_M * T_Mbp_Mi[idx] )
ret.append( diff.vel.Norm() * weights_pos[idx] )
return ret
def get_frame_normal(door):
"""Get the normal of the door frame.
@type door: door_msgs.msg.Door
@rtype: kdl.Vector
"""
# normal on frame
p12 = kdl.Vector(
door.frame_p1.x - door.frame_p2.x,
door.frame_p1.y - door.frame_p2.y,
door.frame_p1.z - door.frame_p2.z)
p12.Normalize()
z_axis = kdl.Vector(0,0,1)
normal = p12 * z_axis
# make normal point in direction we travel through door
direction = kdl.Vector(
door.travel_dir.x,
door.travel_dir.y,
door.travel_dir.z)
if kdl.dot(direction, normal) < 0:
normal = normal * -1.0
return normal
def calc_R(rx, ry, rz):
R_7_M = PyKDL.Frame(PyKDL.Rotation.EulerZYX(rx, ry, rz))
ret = []
for idx in range(0,len(T_7bo_7i)):
diff = PyKDL.diff( T_7bo_7i[idx] * R_7_M, R_7_M * T_Mbo_Mi[idx] )
ret.append( diff.rot.Norm() * weights_ori[idx] )
return ret
def sampleOptimalZAngle(moveItInterface, R_bin_sample, R_eef_gripper_base, desiredZAxis, numAngles):
'''
Find optimal angle for rotating a pose sample around Z-axis so that the gripper base's
z axis aligns as much as possible with the desired z axis w.r.t the bin.
@param R_bin_sample: rotation of the sample w.r.t. bin frame (kdl.Rotation)
@param R_eef_gripper_base: rotation of the eef (tool-tip) w.r.t. gripper base frame (kdl.Rotation)
@return: optimal angle (float)
'''
z_bin_gripper_base_desired = kdl.Vector(*(desiredZAxis))
# evaluate for a series of rotations around z axis
thetas_z = numpy.linspace(0, 2 * numpy.pi, numAngles)
dot_products = numpy.array([])
for angle in thetas_z:
moveItInterface.checkPreemption()
R_bin_sample_rotated = R_bin_sample * kdl.Rotation.RotZ(angle)
# resulting Z-axis of gripper_base
z_bin_gripper_base = R_bin_sample_rotated * R_eef_gripper_base * kdl.Vector(0.0, 0.0, 1.0)
dot_products = numpy.append(dot_products, kdl.dot(z_bin_gripper_base, z_bin_gripper_base_desired))
optimal_angle = thetas_z[dot_products.argmax()]
return optimal_angle
def compute(self):
vec1 = self.vec1.compute()
vec2 = self.vec2.compute()
denom = vec1.Norm() * vec2.Norm()
if denom == 0:
return 0
else:
return kdl.dot(vec1, vec2) / denom
def compute(self):
vec = self.vec.compute()
ref = self.ref.compute()
denom = ref.dir.Norm()**2
if denom == 0:
res = kdl.Vector()
else:
res = ref.dir * (kdl.dot(ref.dir, vec.dir) / denom)
return LocatedVector(vec.pos, res)
def determine_points_of_interest(self, center_frame, z_max, convex_hull):
"""
Determine candidates for place poses
:param center_frame: kdl.Frame, center of the Entity to place on top of
:param z_max: float, height of the entity to place on, w.r.t. the entity
:param convex_hull: [kdl.Vector], convex hull of the entity
:return: [FrameStamped] of candidates for placing
"""
points = []
if len(convex_hull) == 0:
rospy.logerr('determine_points_of_interest: Empty convex hull')
return []
# Convert convex hull to map frame
ch = offsetConvexHull(convex_hull, center_frame)
# Loop over hulls
self.marker_array.markers = []
for i in xrange(len(ch)):
j = (i + 1) % len(ch)
dx = ch[j].x() - ch[i].x()
dy = ch[j].y() - ch[i].y()
length = kdl.diff(ch[j], ch[i]).Norm()
d = self._edge_distance
while d < (length - self._edge_distance):
# Point on edge
xs = ch[i].x() + d / length * dx
ys = ch[i].y() + d / length * dy
# Shift point inwards and fill message
fs = kdl_frame_stamped_from_XYZRPY(x=xs - dy / length * self._edge_distance,
y=ys + dx / length * self._edge_distance,
z=center_frame.p.z() + z_max,
frame_id="/map")
# It's nice to put an object on the middle of a long edge. In case of a cabinet, e.g., this might
# prevent the robot from hitting the cabinet edges
# print "Length: {}, edge score: {}".format(length, min(d, length-d))
setattr(fs, 'edge_score', min(d, length-d))
points += [fs]
self.marker_array.markers.append(self.create_marker(fs.frame.p.x(), fs.frame.p.y(), fs.frame.p.z()))
# ToDo: check if still within hull???
d += self._spacing
self.marker_pub.publish(self.marker_array)
return points
def axis_marker(tw, id = 0, ns = 'twist'):
""" make a marker message showing the instantaneous
rotation axis of a twist message"""
t = kdl.Twist(kdl.Vector(tw.linear.x, tw.linear.y, tw.linear.z),
kdl.Vector(tw.angular.x, tw.angular.y, tw.angular.z))
try:
(x, rot) = listener.lookupTransform(target_frame, ref_frame, rospy.Time(0))
(trans, rotp) = listener.lookupTransform(target_frame, ref_point, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
print 'tf exception!'
return marker.create(id=id, ns=ns, action=Marker.DELETE)
# put the twist in the right frame
f = posemath.fromTf( (trans, rot) )
t = f*t
direction = t.rot
location = t.rot * t.vel / kdl.dot(t.rot, t.rot)
lr = t.rot.Norm()
lt = t.vel.Norm()
m = marker.create(id=id, ns=ns, type=Marker.CYLINDER)
if lr < 0.0001 and lt < 0.0001:
return marker.create(id=id, ns=ns, action=Marker.DELETE)
if lr < 0.001:
# very short axis, assume linear movement
location = kdl.Vector(0,0,0)
direction = t.vel
if lt < min_length:
direction *= min_length / lt
m.type = Marker.CUBE
m = marker.align(m, location, location + direction, 0.02)
elif lr < min_length:
direction = direction / lr * min_length
m = marker.align(m, location - direction, location + direction, 0.02)
elif lr > max_length:
direction = direction / lr * max_length
m = marker.align(m, location - direction, location + direction, 0.02)
else:
#BAH! How do I make this better?
m = marker.align(m, location - direction, location + direction, 0.02)
m.header.frame_id = target_frame
m.frame_locked = True
if(use_colors):
m.color = colors[id % len(colors)]
else:
m.color = ColorRGBA(0.3, 0.3, 0.3, 1)
return m
def calc_R(rx, ry, rz):
R_mean = PyKDL.Frame(PyKDL.Rotation.EulerZYX(rx, ry, rz))
diff = []
for r in R:
diff.append(PyKDL.diff( R_mean, r ))
ret = []
for idx in range(0, len(diff)):
rot_err = diff[idx].rot.Norm()
ret.append(rot_err * wg[idx] / wg_sum)
return ret
def updateCom2(T_B_O, T_B_O_2, contacts_O, com_pt, com_weights):
diff = PyKDL.diff(T_B_O, T_B_O_2)
up_v = PyKDL.Vector(0, 0, 1)
n_v = diff.rot * up_v
n_O = T_B_O.Inverse() * n_v
n_O_2 = T_B_O_2.Inverse() * n_v
# get all com points that lies in the positive direction from the most negative contact point with respect to n_v in T_B_O and T_B_O_2
# get the minimum contact point for n_O
min_c_dot = None
for c in contacts_O:
dot = PyKDL.dot(c, n_O)
if min_c_dot == None or dot < min_c_dot:
min_c_dot = dot
# get all com points that lies in the positive direction from min_c_dot
com_2_idx = []
for c_idx in range(0, len(com_pt)):
c = com_pt[c_idx]
dot = PyKDL.dot(c, n_O)
if dot > min_c_dot:
com_2_idx.append(c_idx)
# get the minimum contact point for n_O
min_c_dot = None
for c in contacts_O:
dot = PyKDL.dot(c, n_O_2)
if min_c_dot == None or dot < min_c_dot:
min_c_dot = dot
# get all com points that lies in the positive direction from min_c_dot
com_3_idx = []
for c_idx in com_2_idx:
c = com_pt[c_idx]
dot = PyKDL.dot(c, n_O_2)
if dot > min_c_dot:
com_3_idx.append(c_idx)
for c_idx in range(0, len(com_pt)):
com_weights[c_idx] -= 1
for c_idx in com_3_idx:
com_weights[c_idx] += 2
def checkGoal(self, q):
# interpolate trajectory (in the cartesian space)
self.openrave.robot_rave.SetDOFValues(q, self.dof_indices)
link_E = self.openrave.robot_rave.GetLink("right_HandPalmLink")
T_World_E = conv.OpenraveToKDL(link_E.GetTransform())
T_B_E = self.openrave.T_World_Br.Inverse() * T_World_E
for T_B_Eg in self.goals_T_B_E:
diff = PyKDL.diff(T_B_Eg, T_B_E)
if diff.vel.Norm() <= 0.02 and diff.rot.Norm() <= 10.0/180.0*math.pi:
return True
return False
def setParams(self, param):
'''
0-15: homogeneous goal matrix
16: slow down distance
'''
gpos = matrix([param[0:4], param[4:8], param[8:12], param[12:16]])
self.gp = gpos[0:3, 3].T.A[0]
self.dToAttractor = length(self.p - self.gp)
self.slowDownDistance = param[16]
for i in range(3):
for j in range(3):
self.gframe.M[i, j] = gpos[i, j]
self.tw = kdl.diff(self.frame, self.gframe)
def within_tolerance(self, tip_pose, target_pose, scale=1.0):
# compute cartesian command error
tip_frame = fromTf(tip_pose)
target_frame = fromTf(target_pose)
twist_err = kdl.diff(tip_frame, target_frame)
linear_err = twist_err.vel.Norm()
angular_err = twist_err.rot.Norm()
#print("linear: %g, angular %g" % (linear_err, angular_err))
# decide if planning is needed
return linear_err < scale*self.linear_err_threshold and angular_err < scale*self.angular_err_threshold
def getMovementTime2(self, T_B_Wd1, T_B_Wd2, max_v_l = 0.1, max_v_r = 0.2):
twist = PyKDL.diff(T_B_Wd1, T_B_Wd2, 1.0)
v_l = twist.vel.Norm()
v_r = twist.rot.Norm()
print "v_l: %s v_r: %s"%(v_l, v_r)
f_v_l = v_l/max_v_l
f_v_r = v_r/max_v_r
if f_v_l > f_v_r:
duration = f_v_l
else:
duration = f_v_r
if duration < 0.5:
duration = 0.5
return duration
def markers(self):
''' get the markers for visualization '''
self._compute_lengths()
frame = self._transform()
mrks = []
pos = kdl.Vector(0,0,0)
l_index = 0
for i,t in enumerate(self.jac):
twist = frame*t
lr = twist.rot.Norm()
lv = twist.vel.Norm()
if lr < self.eps and lv < self.eps:
# null twist, make a delete marker
mrks.append(marker.create(id=i, ns=self.name+str(i), action=Marker.DELETE))
continue
if lr < self.eps:
# pure translational twist
location = pos
direction = twist.vel / lv * self.lengths[l_index]
m = marker.create(id=i, ns=self.name+str(i), type=Marker.CUBE)
m = marker.align(m, location, location + direction, self.width)
l_index += 1
frame.p = frame.p - direction # move target point to end of this 'axis'
pos += direction # remember current end point
else:
# rotational twist
location = twist.rot * twist.vel / kdl.dot(twist.rot, twist.rot) + pos
if self.independent_directions:
direction = twist.rot * self.rot_length / lr
else:
# take axis from interaction matrix and transform to location
dir_H = self.H[i].rot - location*self.H[i].vel
direction = dir_H * self.rot_length / dir_H.Norm()
m = marker.create(id=i, ns=self.name+str(i), type=Marker.CYLINDER)
m = marker.align(m, location - direction, location + direction, self.width)
m.color = self._color(i)
m.header.frame_id = self.target_frame
mrks.append(m)
return mrks
def __init__(self): self.frame=kdl.Frame() self.gframe=kdl.Frame() self.gp=array([0,0,0]) self.p=array([0,0,0]) self.dToAttractor=0 self.slowDownDistance=0.05 #normal value 0.03 self.tw=kdl.diff(self.frame,self.gframe) self.rot_factor=1.0 self.min_rot=0.09 self.stopDistance=0.005 self.r_stopDistance=0.03 self.slowDownTime=0.1 self.slowDowninitTime=time.time() self.time=time.time() self.filter=[1.0]*5 self.filterC=array([0.05,0.3,0.3,0.3,0.05])
def getVector(self,pos):
''' 0-15: position'''
pos=matrix([pos[0:4],pos[4:8],pos[8:12],pos[12:16]])
self.p=pos[0:3,3].T.A[0]
r=pos[0:3,0:3]
#Cartesian vector
self.dToAttractor=length(self.p-self.gp)
if self.dToAttractor==0:
P=array([0,0,0])
else:
P=-(self.p-self.gp)/self.dToAttractor
#todo: to improve stability we could decay near the attraction point
#rotational twist
for i in range(3):
for j in range(3):
self.frame.M[i,j]=r[i,j]
self.tw=kdl.diff(self.frame,self.gframe)
return concatenate((P,array([self.tw.rot[0],self.tw.rot[1],self.tw.rot[2]])),0)
def inertia(self, joint_values=None):
inertia = PyKDL.JntSpaceInertiaMatrix(self._num_jnts)
self._dyn_kdl.JntToMass(self.joints_to_kdl('positions', joint_values),
inertia)
return self.kdl_to_mat(inertia)
def setup_kdl_chain(self):
for dh in self.dh_params:
self.kine_chain.addSegment(PyKDL.Segment(PyKDL.Joint(PyKDL.Vector(), PyKDL.Vector(0, 0, dh[4]),
PyKDL.Joint.RotAxis),
PyKDL.Frame().DH(dh[0], dh[1], dh[2], dh[3])))
def TrotZ(theta): s = math.sin(theta) c = math.cos(theta) T = kdl.Frame(kdl.Rotation(c, -s, 0, s, c, 0, 0, 0, 1), kdl.Vector(0, 0, 0)) return T
#! /usr/bin/python
import rospy
import json
import PyKDL as kdl
import os
from sensor_msgs.msg import *
from geometry_msgs.msg import *
from tf.transformations import *
from visualization_msgs.msg import Marker
kdlChain = kdl.Chain()
frame = kdl.Frame()
print("przed")
print(kdlChain)
print("po")
with open(os.path.dirname(os.path.realpath(__file__)) + '/../dh.json',
'r') as file:
params = json.loads(file.read())
def callback(data):
kdlChain = kdl.Chain()
frame = kdl.Frame()
with open(
os.path.dirname(os.path.realpath(__file__)) + '/../dh.json',
'r') as file:
params = json.loads(file.read())
matrices = {}
for key in sorted(params.keys()):
def run_jaw_move(self):
print_id('starting jaw move')
# try to open and close with the cartesian part of the arm in different modes
print_id('close and open without other move command')
input(" Press Enter to continue...")
print_id('closing (1)')
self.arm.jaw.close().wait()
print_id('opening (2)')
self.arm.jaw.open().wait()
print_id('closing (3)')
self.arm.jaw.close().wait()
print_id('opening (4)')
self.arm.jaw.open().wait()
# try to open and close with a joint goal
print_id('close and open with joint move command')
input(" Press Enter to continue...")
print_id('closing and moving up (1)')
self.arm.jaw.close().wait(is_busy=True)
self.arm.insert_jp(0.1).wait()
print_id('opening and moving down (2)')
self.arm.jaw.open().wait(is_busy=True)
self.arm.insert_jp(0.15).wait()
print_id('closing and moving up (3)')
self.arm.jaw.close().wait(is_busy=True)
self.arm.insert_jp(0.1).wait()
print_id('opening and moving down (4)')
self.arm.jaw.open().wait(is_busy=True)
self.arm.insert_jp(0.15).wait()
print_id('close and open with cartesian move command')
input(" Press Enter to continue...")
# try to open and close with a cartesian goal
self.prepare_cartesian()
initial_cartesian_position = PyKDL.Frame()
initial_cartesian_position.p = self.arm.setpoint_cp().p
initial_cartesian_position.M = self.arm.setpoint_cp().M
goal = PyKDL.Frame()
goal.p = self.arm.setpoint_cp().p
goal.M = self.arm.setpoint_cp().M
# motion parameters
amplitude = 0.05 # 5 cm
# first motion
goal.p[0] = initial_cartesian_position.p[0] - amplitude
goal.p[1] = initial_cartesian_position.p[1]
print_id('closing and moving right (1)')
self.arm.move_cp(goal).wait(is_busy=True)
self.arm.jaw.close().wait()
# second motion
goal.p[0] = initial_cartesian_position.p[0] + amplitude
goal.p[1] = initial_cartesian_position.p[1]
print_id('opening and moving left (1)')
self.arm.move_cp(goal).wait(is_busy=True)
self.arm.jaw.open().wait()
# back to starting point
goal.p[0] = initial_cartesian_position.p[0]
goal.p[1] = initial_cartesian_position.p[1]
print_id('moving back (3)')
self.arm.move_cp(goal).wait()
def convert_msg_point_to_kdl_vector(point):
return PyKDL.Vector(point.x, point.y, point.z)
def gravity_compensation(self, world):
m = world.get_object_mass(self._object_name)
g = world.get_gravity()
return kdl.Wrench(-m * g, kdl.Vector())
snake = u2c.URDFparser() snake.from_file(path_to_urdf) #pybullet sim = pb.connect(pb.DIRECT) snake_pb = pb.loadURDF(path_to_urdf, useFixedBase=True, flags = pb.URDF_USE_INERTIA_FROM_FILE) #joint info jointlist, names, q_max, q_min = snake.get_joint_info(root, tip) n_joints = snake.get_n_joints(root, tip) #declarations #kdl q_kdl = kdl.JntArray(n_joints) qdot_kdl = kdl.JntArray(n_joints) gravity_kdl = kdl.Vector() gravity_kdl[2] = -9.81 C_kdl = kdl.JntArray(n_joints) g_kdl = kdl.JntArray(n_joints) #u2c & pybullet q = [None]*n_joints qdot = [None]*n_joints zeros_pb = [None]*n_joints gravity_u2c = [0, 0, -9.81] g_sym = snake.get_gravity_rnea(root, tip, gravity_u2c) C_sym = snake.get_coriolis_rnea(root, tip)
def process(self, msg):
if not self.thread_lock.acquire(False):
return
#msg = tf2_sensor_msgs.do_transform_cloud(msg, self.trans)
#msg.header.frame_id = "base_link"
#self.pub.publish(msg)
pc = ros_to_pcl(msg)
pc = XYZRGB_to_XYZ(pc)
req = SegmentSceneRequest()
req.dist_threshold = 0.07
req.angle_threshold = 0.1
req.plane_dist_threshold = 0.05
req.min_plane_size = .25
req.max_floor_height = 5.
req.cluster_tolerance = 0.09
req.min_cluster_size = 0.05
req.max_cluster_size = 10
pts = np.asarray(pc).tolist()
#import pdb; pdb.set_trace()
for (x, y, z) in pts:
if np.isnan(x): continue
req.points.append(Point(x, y, z))
result = self.srv.call(req)
detreq = DetectModelsRequest()
detreq.models = ["soda"]
detreq.max_dists_xy = [2]
detreq.max_dists_z = [2]
import pdb
pdb.set_trace()
for wall in result.walls:
poly = Polygon()
pointsvect = [
self.T * PyKDL.Vector(p.x, p.y, p.z) for p in wall.points
]
points = np.array([[p[0], p[1], p[2]] for p in pointsvect])
#import pdb; pdb.set_trace()
try:
ch = CHull(points)
except:
continue
max_x, max_y, max_z = ch.max_pts()
min_x, min_y, min_z = ch.min_pts()
#if min_z < 0: continue
avg_z = np.mean(points, axis=0)[2]
ch_xy = CHull(points[:, :2])
#surf_pts = [self.T.Inverse()*PyKDL.Vector(x, y, avg_z) for (x,y) in ch_xy.get_vertices()]
#ch_pts = [Point(p[0], p[1], p[2]) for p in surf_pts]
ch_pts = [Point(x, y, avg_z) for (x, y) in ch_xy.get_vertices()]
poly.points = ch_pts
detreq.surface_polygons.append(poly)
ps = PolygonStamped(self.lastHeader, poly)
ps.header.frame_id = "base_link"
self.tablepub.publish(ps)
tmp_pose = Pose()
tmp_pose.orientation.x = 1
detreq.initial_poses = [tmp_pose]
detreq.header = self.lastHeader
detreq.timeout = rospy.Duration(5)
detreq.initial_poses = [Pose()]
detreq.header.frame_id = "base_link"
try:
result = self.detsrv.call(detreq)
print "success!"
print result
except:
print "failed"
"""
fil = pc.make_passthrough_filter()
fil.set_filter_field_name("z")
fil.set_filter_limits(.5, 1.5)
cloud_filtered = fil.filter()
seg = cloud_filtered.make_segmenter_normals(ksearch=50)
seg.set_optimize_coefficients(True)
seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE)
seg.set_normal_distance_weight(0.05)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_max_iterations(300)
seg.set_distance_threshold(0.03)
indices, model = seg.segment()
print(model)
cloud_plane = cloud_filtered.extract(indices, negative=False)
new_msg = self.pclToMsg(cloud_plane)
self.tablepub.publish(new_msg)
cloud_cyl = cloud_filtered.extract(indices, negative=True)
new_msg = self.pclToMsg(cloud_cyl)
self.nottablepub.publish(new_msg)
seg = cloud_cyl.make_segmenter_normals(ksearch=50)
seg.set_optimize_coefficients(True)
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_normal_distance_weight(0.1)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_max_iterations(10000)
seg.set_distance_threshold(0.05)
#seg.set_radius_limits(0, 0.5)
indices, model = seg.segment()
print(model)
cloud_cylinder = cloud_cyl.extract(indices, negative=False)
new_msg = self.pclToMsg(cloud_cylinder)
self.pub.publish(new_msg)
"""
self.thread_lock.release()
def forward_velocity_kinematics(self,joint_velocities=None):
end_frame = PyKDL.FrameVel()
self._fk_v_kdl.JntToCart(self.joints_to_kdl('velocities',joint_velocities),
end_frame)
return end_frame.GetTwist()
def isPointInRange(P0,basePos,a,b) :
x , y, z = intersection(P0,basePos.p, a,b)
if (ifEqual(a.y(),b.y()) and a.x() <= x and b.x() >= x) or ( ifEqual(a.x(),b.x()) and a.y() <= y and b.y() >= y ) :
return PyKDL.Vector(x,y,z)
else :
return 0
# VELMA IS READY FOR MOVING
print "Preparing for grabing jar."
planAndExecute(q_map_1)
#Left arm is ready
jarFrame = velma.getTf("B", "jar")
jarX, jarY, jarZ = jarFrame.p
actual_pos = velma.getLastJointState()
rotZ = actual_pos[1]['torso_0_joint'] + math.pi
print("podchodze do celu")
Rot = PyKDL.Rotation.RotZ(rotZ)
P2_global = PyKDL.Vector(jarX-0.25,jarY,jarZ+0.1)
T_B_Trd = PyKDL.Frame(Rot, P2_global)
velma.moveCartImpLeft([T_B_Trd], [4.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5)
if velma.waitForEffectorLeft() != 0:
exitError(17)
rospy.sleep(0.5)
#close fingers left hand and grab jar
fin_angle = 0.43*math.pi
dest_q = [fin_angle,fin_angle,fin_angle,0]
velma.moveHandLeft(dest_q, [1,1,1,1], [2000,2000,2000,2000], 1000, hold=True)
if velma.waitForHandLeft() != 0:
exitError(6)
rospy.sleep(0.5)
import dvrk
import PyKDL as pk
import time
import math
arm = dvrk.mtm('MTMR')
# arm.home()
# arm.dmove(pk.Vector(-0.001, 0.0, 0.0))
# arm.dmove(pk.Vector(0.001, 0.0, 0.0))
# print "moving forward"
#
# for i in range(1, 41):
# arm.dmove(pk.Vector(-0.0005, 0.0, 0.0))
#
# time.sleep(2)
#
# print "moving backward"
# for i in range(1, 41):
# arm.dmove(pk.Vector(0.0005, 0.0, 0.0))
#
# print "movement completed"
r = pk.Rotation()
r.DoRotZ(math.pi / 6.0)
arm.dmove(r)
def jacobian(self, joint_values=None):
jacobian = PyKDL.Jacobian(self._num_jnts)
self._jac_kdl.JntToJac(self.joints_to_kdl('positions', joint_values),
jacobian)
return self.kdl_to_mat(jacobian)
def list_to_vect(li):
return kd.Vector(li[0], li[1], li[2])
def inverse_kinematics(self,
position,
orientation=None,
seed=None,
min_joints=None,
max_joints=None,
w_x=None,
w_q=None,
maxiter=500,
eps=1.0e-6,
with_st=False):
if w_x is None and w_q is None:
ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
else:
ik_v_kdl = PyKDL.ChainIkSolverVel_wdls(self._arm_chain)
if w_x is not None: ik_v_kdl.setWeightTS(w_x) #TS = Task Space
if w_q is not None: ik_v_kdl.setWeightJS(w_q) #JS = Joint Space
pos = PyKDL.Vector(position[0], position[1], position[2])
if orientation is not None:
rot = PyKDL.Rotation()
rot = rot.Quaternion(orientation[0], orientation[1],
orientation[2], orientation[3])
# Populate seed with current angles if not provided
seed_array = PyKDL.JntArray(self._num_jnts)
if seed is not None:
seed_array.resize(len(seed))
for idx, jnt in enumerate(seed):
seed_array[idx] = jnt
else:
seed_array = self.joints_to_kdl('positions')
# Make IK Call
if orientation is not None:
goal_pose = PyKDL.Frame(rot, pos)
else:
goal_pose = PyKDL.Frame(pos)
result_angles = PyKDL.JntArray(self._num_jnts)
# Make IK solver with joint limits
if min_joints is None: min_joints = self.joint_limits_lower
if max_joints is None: max_joints = self.joint_limits_upper
mins_kdl = PyKDL.JntArray(len(min_joints))
for idx, jnt in enumerate(min_joints):
mins_kdl[idx] = jnt
maxs_kdl = PyKDL.JntArray(len(max_joints))
for idx, jnt in enumerate(max_joints):
maxs_kdl[idx] = jnt
ik_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl,
maxs_kdl, self._fk_p_kdl,
ik_v_kdl, maxiter, eps)
if ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
result = np.array(list(result_angles))
if with_st: return True, result
else: return result
else:
if with_st:
result = np.array(list(result_angles))
return False, result
else:
return None
class PickAndPlaceState(Enum):
OPEN_JAW = 1,
APPROACH_OBJECT = 2,
GRAB_OBJECT = 3,
CLOSE_JAW = 4,
APPROACH_DEST = 5,
DROP_OBJECT = 6,
HOME = 7,
DONE = 8
VECTOR_EPS = 0.005
DOWN_JAW_ORIENTATION = PyKDL.Rotation.RPY(math.pi, 0, -math.pi / 2.0)
PSM_HOME_POS = PyKDL.Vector(0., 0., -0.1)
def vector_eps_eq(lhs, rhs):
return bool((lhs - rhs).Norm() < 0.005)
class PickAndPlaceStateMachine:
def jaw_fully_open(self):
return True if self.psm.get_current_jaw_position(
) >= math.pi / 3 else False
def jaw_fully_closed(self):
return True if self.psm.get_current_jaw_position() <= 0 else False
def _open_jaw(self):
def point2kdl(point):
return kdl.Vector(point.x, point.y, point.z)
type=float,
help="y-coordinate (in map) of the imaginary object")
parser.add_argument("z",
type=float,
help="z-coordinate (in map) of the imaginary object")
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
rospy.init_node("test_grasping")
robot = get_robot(args.robot)
entity_id = "test_item"
pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0),
kdl.Vector(args.x, args.y, args.z)),
frame_id="/map")
robot.ed.update_entity(id=entity_id, frame_stamped=pose)
shape = RightPrism([
kdl.Vector(0, 0, 0),
kdl.Vector(0, 0.05, 0),
kdl.Vector(0.05, 0.05, 0),
kdl.Vector(0.05, 0, 0)
], -0.1, 0.1)
item = Entity(entity_id, "test_type", pose.frame_id, pose.frame, shape,
None, None, None)
item = ds.Designator(item)
arm = ds.UnoccupiedArmDesignator(robot,
arm_properties={
def _set_arm_dest(self, dest):
if self.log_verbose:
loginfo("Setting {} dest to {}".format(self.psm.name(), dest))
if self.psm.get_desired_position().p != dest:
self.psm.move(PyKDL.Frame(DOWN_JAW_ORIENTATION, dest),
blocking=False)
def transform_to_kdl(self,t):
return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y,
t.transform.rotation.z, t.transform.rotation.w),
PyKDL.Vector(t.transform.translation.x,
t.transform.translation.y,
t.transform.translation.z))
def __init__(self,
limb,
ee_frame_name="panda_link8",
additional_segment_config=None,
description=None):
if description is None:
self._franka = URDF.from_parameter_server(key='robot_description')
else:
self._franka = URDF.from_xml_file(description)
self._kdl_tree = kdl_tree_from_urdf_model(self._franka)
if additional_segment_config is not None:
for c in additional_segment_config:
q = quaternion.from_rotation_matrix(c["origin_ori"]).tolist()
kdl_origin_frame = PyKDL.Frame(
PyKDL.Rotation.Quaternion(q.x, q.y, q.z, q.w),
PyKDL.Vector(*(c["origin_pos"].tolist())))
kdl_sgm = PyKDL.Segment(c["child_name"],
PyKDL.Joint(c["joint_name"]),
kdl_origin_frame,
PyKDL.RigidBodyInertia())
self._kdl_tree.addSegment(kdl_sgm, c["parent_name"])
self._base_link = self._franka.get_root()
self._tip_link = ee_frame_name
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
self._limb_interface = limb
self._joint_names = deepcopy(self._limb_interface.joint_names())
self._num_jnts = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def get_jacobian(self, joint):
jac = PyKDL.Jacobian(self.kine_chain.getNrOfJoints())
self.jac_calc.JntToJac(joint, jac)
return jac
# The ONLY objects the collision detection software is aware
# of are itself & the floor.
if __name__ == '__main__':
rospy.init_node('ar_detect_and_grab')
cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
listener = tf.TransformListener()
try:
listener.waitForTransform('/base_link', '/card_reader', rospy.Time(0),
rospy.Duration(4.0))
(trans, rot) = listener.lookupTransform('/base_link', '/card_reader',
rospy.Time(0))
res = Pose(Point(*trans), Quaternion(*rot))
frame_gripper_link = PyKDL.Frame(PyKDL.Vector(-0.166, 0.0, 0.0))
(trans_res, rot_res) = tf_conversions.posemath.toTf(
tf_conversions.posemath.fromMsg(res) * frame_gripper_link)
frame_prep_link = PyKDL.Frame(PyKDL.Vector(-0.166, 0.0, 0.1))
(trans_prep, rot_prep) = tf_conversions.posemath.toTf(
tf_conversions.posemath.fromMsg(res) * frame_prep_link)
except (tf.LookupException, tf.ConnectivityException):
print 'couldnt get transform to handle'
# Create move group interface for a fetch robot
move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
def get_objects_and_img(left_image_msg, right_image_msg, stereo_cam_model,
cam_to_world_tf):
# this gets the position of the red ball thing in the camera frame
# and the image with X's on the desired features
fp = feature_processor(FEAT_PATHS)
left_feats, left_frame = fp.FindImageFeatures(left_image_msg)
right_feats, right_frame = fp.FindImageFeatures(right_image_msg)
# discard features with image y > bowl y
left_bowl = None
for left_feat in left_feats:
if left_feat.type == FeatureType.BOWL:
left_bowl = left_feat
left_feats = filter(lambda feat: feat.pos[1] >= left_bowl.pos[1],
left_feats)
left_feats.sort(key=lambda feat: feat.pos[1], reverse=False)
right_bowl = None
for right_feat in right_feats:
if right_feat.type == FeatureType.BOWL:
right_bowl = right_feat
right_feats = filter(lambda feat: feat.pos[1] >= right_bowl.pos[1],
right_feats)
right_feats.sort(key=lambda feat: feat.pos[1], reverse=False)
matched_feats = []
for left_feat in left_feats:
same_color_feats = filter(
lambda right_feat: right_feat.color == left_feat.color,
right_feats)
if not same_color_feats:
rospy.logwarn(
"Failed to match left detection {} to a right detection!".
format(left_feat))
continue
matched_feats.append((left_feat,
min(same_color_feats,
key=lambda right_feat: (right_feat.pos[0] - left_feat.pos[0]) ** 2 \
+ (right_feat.pos[1] - left_feat.pos[1]) ** 2)))
objects = []
for left_feat, right_feat in matched_feats:
disparity = abs(left_feat.pos[0] - right_feat.pos[0])
pos_cv = stereo_cam_model.projectPixelTo3d(left_feat.pos,
float(disparity))
# there's a fixed rotation to convert this to the camera coordinate frame
pos_cam = np.matmul(CV_TO_CAM_FRAME_ROT, pos_cv)
pos = None
if cam_to_world_tf is not None:
pos = cam_to_world_tf * PyKDL.Vector(*pos_cam)
else:
pos = PyKDL.Vector(*pos_cam)
if left_feat.color != right_feat.color:
rospy.loginfo("Color mismatch between left and right detection")
objects.append(Object3d(pos, left_feat.type, left_feat.color))
return objects, np.hstack((left_frame, right_frame))
def linkStatesCallback(self, msg):
self.getNameOrder(msg.name)
q_world = PyKDL.Rotation.Quaternion(msg.pose[0].orientation.x, msg.pose[0].orientation.y, msg.pose[0].orientation.z, msg.pose[0].orientation.w)
q_base = PyKDL.Rotation.Quaternion(msg.pose[self.order[0]].orientation.x, msg.pose[self.order[0]].orientation.y, msg.pose[self.order[0]].orientation.z, msg.pose[self.order[0]].orientation.w)
if self.Gtype == 'reflex' or self.Gtype == 'model_O':
q_1_1 = PyKDL.Rotation.Quaternion(msg.pose[self.order[1]].orientation.x, msg.pose[self.order[1]].orientation.y, msg.pose[self.order[1]].orientation.z, msg.pose[self.order[1]].orientation.w)
q_1_2 = PyKDL.Rotation.Quaternion(msg.pose[self.order[2]].orientation.x, msg.pose[self.order[2]].orientation.y, msg.pose[self.order[2]].orientation.z, msg.pose[self.order[2]].orientation.w)
q_1_3 = PyKDL.Rotation.Quaternion(msg.pose[self.order[3]].orientation.x, msg.pose[self.order[3]].orientation.y, msg.pose[self.order[3]].orientation.z, msg.pose[self.order[3]].orientation.w)
q_2_1 = PyKDL.Rotation.Quaternion(msg.pose[self.order[4]].orientation.x, msg.pose[self.order[4]].orientation.y, msg.pose[self.order[4]].orientation.z, msg.pose[self.order[4]].orientation.w)
q_2_2 = PyKDL.Rotation.Quaternion(msg.pose[self.order[5]].orientation.x, msg.pose[self.order[5]].orientation.y, msg.pose[self.order[5]].orientation.z, msg.pose[self.order[5]].orientation.w)
q_2_3 = PyKDL.Rotation.Quaternion(msg.pose[self.order[6]].orientation.x, msg.pose[self.order[6]].orientation.y, msg.pose[self.order[6]].orientation.z, msg.pose[self.order[6]].orientation.w)
q_3_2 = PyKDL.Rotation.Quaternion(msg.pose[self.order[7]].orientation.x, msg.pose[self.order[7]].orientation.y, msg.pose[self.order[7]].orientation.z, msg.pose[self.order[7]].orientation.w)
q_3_3 = PyKDL.Rotation.Quaternion(msg.pose[self.order[8]].orientation.x, msg.pose[self.order[8]].orientation.y, msg.pose[self.order[8]].orientation.z, msg.pose[self.order[8]].orientation.w)
# Velocity should be added here
else:
q_1_1 = PyKDL.Rotation.Quaternion(msg.pose[self.order[1]].orientation.x, msg.pose[self.order[1]].orientation.y, msg.pose[self.order[1]].orientation.z, msg.pose[self.order[1]].orientation.w)
q_1_2 = PyKDL.Rotation.Quaternion(msg.pose[self.order[2]].orientation.x, msg.pose[self.order[2]].orientation.y, msg.pose[self.order[2]].orientation.z, msg.pose[self.order[2]].orientation.w)
q_2_1 = PyKDL.Rotation.Quaternion(msg.pose[self.order[4]].orientation.x, msg.pose[self.order[4]].orientation.y, msg.pose[self.order[4]].orientation.z, msg.pose[self.order[4]].orientation.w)
q_2_2 = PyKDL.Rotation.Quaternion(msg.pose[self.order[5]].orientation.x, msg.pose[self.order[5]].orientation.y, msg.pose[self.order[5]].orientation.z, msg.pose[self.order[5]].orientation.w)
dq_1_1 = msg.twist[self.order[1]].angular.z
dq_1_2 = msg.twist[self.order[2]].angular.z
dq_2_1 = msg.twist[self.order[4]].angular.z
dq_2_2 = msg.twist[self.order[5]].angular.z
if self.Gtype == 'reflex':
# self.joint_angles = [f1_1,f1_2,f1_3,f2_1,f2_2,f2_3,f3_2,f3_3]
self.joint_angles[0] = (q_1_1.Inverse()*q_base).GetEulerZYX()[0]
a = q_1_1.UnitZ()*q_1_2.UnitX()
if a[1] > 0:
self.joint_angles[1] = np.pi/2-np.arccos(PyKDL.dot(q_1_1.UnitZ(), q_1_2.UnitX()))+0.2807829
else:
self.joint_angles[1] = np.pi/2+np.arccos(PyKDL.dot(q_1_1.UnitZ(), q_1_2.UnitX()))+0.2807829
self.joint_angles[2] = (q_1_3.Inverse()*q_1_2).GetEulerZYX()[1]
self.joint_angles[3] = (q_2_1.Inverse()*q_base).GetEulerZYX()[0]
a = q_2_1.UnitZ()*q_2_2.UnitX()
if a[1] > 0:
self.joint_angles[4] = np.pi/2-np.arccos(PyKDL.dot(q_2_1.UnitZ(), q_2_2.UnitX()))+0.2807829
else:
self.joint_angles[4] = np.pi/2+np.arccos(PyKDL.dot(q_2_1.UnitZ(), q_2_2.UnitX()))+0.2807829
self.joint_angles[5] = (q_2_3.Inverse()*q_2_2).GetEulerZYX()[1]
a = q_base.UnitZ()*q_3_2.UnitX()
if a[1] < 0:
self.joint_angles[6] = np.pi/2-np.arccos(PyKDL.dot(q_base.UnitZ(), q_3_2.UnitX()))+0.2807829
else:
self.joint_angles[6] = np.pi/2+np.arccos(PyKDL.dot(q_base.UnitZ(), q_3_2.UnitX()))+0.2807829
self.joint_angles[7] = (q_3_3.Inverse()*q_3_2).GetEulerZYX()[1]
if self.Gtype == 'model_O':
# self.joint_angles = [f1_1,f1_2,f1_3,f2_1,f2_2,f2_3,f3_2,f3_3]
self.joint_angles[0] = np.mod(np.pi-(q_1_1.Inverse()*q_base).GetEulerZYX()[2], np.pi)
self.joint_angles[1] = -(q_1_2.Inverse()*q_1_1).GetEulerZYX()[0]
self.joint_angles[2] = -(q_1_3.Inverse()*q_1_2).GetEulerZYX()[0]
self.joint_angles[3] = np.pi+(q_2_1.Inverse()*q_base).GetEulerZYX()[2] # Changes sign based on the rotation of the base - not currently important
self.joint_angles[4] = -(q_2_2.Inverse()*q_2_1).GetEulerZYX()[0]
self.joint_angles[5] = -(q_2_3.Inverse()*q_2_2).GetEulerZYX()[0]
self.joint_angles[6] = -(q_3_2.Inverse()*q_base).GetEulerZYX()[0]
self.joint_angles[7] = -(q_3_3.Inverse()*q_3_2).GetEulerZYX()[0]
if self.Gtype == 'model_T42':
a = (q_1_1.Inverse()*q_base).GetEulerZYX()
if a[0] < 0:
self.joint_angles[0] = -a[1]
else:
self.joint_angles[0] = np.pi+a[1]
self.joint_angles[1] = -(q_1_2.Inverse()*q_1_1).GetEulerZYX()[2]
a = (q_2_1.Inverse()*q_base).GetEulerZYX()
if a[0] < 0:
self.joint_angles[2] = -a[1]
else:
self.joint_angles[2] = np.pi+a[1]
self.joint_angles[3] = -(q_2_2.Inverse()*q_2_1).GetEulerZYX()[2]
# Apply mean filter to joint velocities with windowSize
if self.win.shape[0] < windowSize:
self.joint_vels = np.array([-dq_1_1, -(dq_1_2 - dq_1_1), dq_2_1, dq_2_2 - dq_2_1]) # Currently finger velocity do not compensate base motion
self.win = np.append(self.win, self.joint_vels).reshape(-1, 4)
else:
v = np.array([-dq_1_1, -(dq_1_2 - dq_1_1), dq_2_1, dq_2_2 - dq_2_1]) # Currently finger velocity do not compensate base motion
self.win = np.append(self.win, v).reshape(-1, 4)
self.joint_vels = np.mean(self.win, axis=0)
self.win = np.delete(self.win, 0, axis=0)
self.joint_vels[np.abs(self.joint_vels) <= 0.1e-2] = 0.
go = True
if key == ord("a"): # PRESS 'a' to RESET
arm_right.reset(top_table_pose)
if point is None:
print("No Point")
continue
###################### CONTROL ########################
imgr_center = np.array(imgr.shape[0:2]) / 2
imgr_center = np.flip(imgr_center)
ctrl_x, ctrl_y = -kp_xy * (point - imgr_center)
print("%8.4f, %8.4f" % (ctrl_x, ctrl_y))
# translation vector (wrt image rf)
tr_ctrl = PyKDL.Vector(ctrl_x, ctrl_y, 0)
# rotation from image rf to camera rf
R_img2cam = PyKDL.Rotation().RotZ(math.pi)
frame_cam2eef = arm_right.gettf(frame_id="right_hand_camera",
parent_id="right_gripper")
# rotation from camera rf to gripper rf
R_cam2eef = frame_cam2eef.M
# translation vector (wrt gripper rf)
tr_ctrl = R_cam2eef * R_img2cam * tr_ctrl
# target frame (wrt gripper rf)
frame_ctrl = PyKDL.Frame()
frame_ctrl.p = tr_ctrl
def dict_to_frame(di):
return kd.Frame(kd.Rotation.RPY(di['rx'], di['ry'], di['rz']),
kd.Vector(di['tx'], di['ty'], di['tz']))
#u2c
gantry = u2c.URDFparser()
gantry.from_file(path_to_urdf)
#pybullet
sim = pb.connect(pb.DIRECT)
pbmodel = pb.loadURDF(path_to_urdf,
useFixedBase=True,
flags=pb.URDF_USE_INERTIA_FROM_FILE)
pb.setGravity(0, 0, -9.81)
#joint info
jointlist, names, q_max, q_min = gantry.get_joint_info(root, tip)
n_joints = gantry.get_n_joints(root, tip)
q_kdl = kdl.JntArray(n_joints)
#declarations
q_kdl = kdl.JntArray(n_joints)
#kdl
q_kdl = kdl.JntArray(n_joints)
gravity_kdl = kdl.Vector()
gravity_kdl[2] = -9.81
g_kdl = kdl.JntArray(n_joints)
#u2c & pybullet
q = [None] * n_joints
qdot = [None] * n_joints
zeros_pb = [None] * n_joints
gravity_u2c = [0, 0, -9.81]
g_sym = gantry.get_gravity_rnea(root, tip, gravity_u2c)
def dict_to_vect(di):
return kd.Vector(di['tx'], di['ty'], di['tz'])
def getMovementTime(self, T_B_Wd, max_v_l = 0.1, max_v_r = 0.2):
self.updateTransformations()
twist = PyKDL.diff(self.T_B_W, T_B_Wd, 1.0)
v_l = twist.vel.Norm()
v_r = twist.rot.Norm()
# print "v_l: %s v_r: %s"%(v_l, v_r)
f_v_l = v_l/max_v_l
f_v_r = v_r/max_v_r
if f_v_l > f_v_r:
duration = f_v_l
else:
duration = f_v_r
if duration < 0.2:
duration = 0.2
return duration
