def get_des_twist(self, dT):
""" Get desired twist @ time t, by computing differential of traj_frame @ t=T-1 and t=T
"""
if self.cur_wp_idx > 1:
return PyKDL.diff(pm.fromMatrix(self.traj_list[self.cur_wp_idx - 1]), pm.fromMatrix(self.traj_list[self.cur_wp_idx]), dT)
else: # cur_wp_idx == num_wp
return PyKDL.diff(pm.fromMatrix(self.traj_list[self.cur_wp_idx]), pm.fromMatrix(self.traj_list[self.cur_wp_idx + 1]), dT)
def check_if_new_goal(self, frame):
dist_p = kdl.diff(self.dst_pose.p, frame.p)
dist_M = kdl.diff(self.dst_pose.M, frame.M)
if dist_M[0] != dist_M[0]: #got a nan
rospy.logwarn("Got a NAN in frame.M distance")
return True
dist_p_scalar = kdl.dot(dist_p, dist_p)
dist_M_scalar = kdl.dot(dist_M, dist_M)
if (dist_p_scalar > 0.00001) or (dist_M_scalar > 0.00001):
return True
else:
return False
def compute_num_jac(self, func, ik_config, mat_world_to_obj):
eps = 10**-6
jac = np.zeros((self.palm_dof_dim, self.arm_joints_dof))
for i in xrange(self.arm_joints_dof):
ik_config_plus = np.copy(ik_config)
ik_config_plus[i] += eps
config_plus = func(ik_config_plus, mat_world_to_obj)
ik_config_minus = np.copy(ik_config)
ik_config_minus[i] -= eps
config_minus = func(ik_config_minus, mat_world_to_obj)
# ith_grad = (config_plus[:6] - config_minus[:6]) / (2. * eps)
trans_plus = PyKDL.Vector(config_plus[0], config_plus[1],
config_plus[2])
rot_plus = PyKDL.Rotation.RPY(config_plus[3], config_plus[4],
config_plus[5])
frame_plus = PyKDL.Frame(rot_plus, trans_plus)
trans_minus = PyKDL.Vector(config_minus[0], config_minus[1],
config_minus[2])
rot_minus = PyKDL.Rotation.RPY(config_minus[3], config_minus[4],
config_minus[5])
frame_minus = PyKDL.Frame(rot_minus, trans_minus)
twist = PyKDL.diff(frame_minus, frame_plus, 1.0)
ith_grad = np.array([
twist.vel[0], twist.vel[1], twist.vel[2], twist.rot[0],
twist.rot[1], twist.rot[2]
]) / (2. * eps)
jac[:, i] = ith_grad
return jac
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(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 _update(self, event):
# 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()
# TODO(mam0box) Test for singularities
# 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_joint_efforts(tau)
def odomCallback(self, odom_msg):
current_position = point_msg_to_kdl_vector(odom_msg.pose.pose.position)
if self.active:
self.distance_traveled += kdl.diff(current_position,
self.previous_position).Norm()
self.previous_position = current_position
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 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 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 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 _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 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 moveRight(x, y, z, theta):
modeCart()
print "Moving right wrist to position:", x, y, z, "\n"
T_B_Trd = PyKDL.Frame(PyKDL.Rotation.RPY(0.0, 0.0, theta),
PyKDL.Vector(x, y, z))
if not velma.moveCartImpRight([T_B_Trd], [3.0],
None,
None,
None,
None,
PyKDL.Wrench(PyKDL.Vector(5, 5, 5),
PyKDL.Vector(5, 5, 5)),
start_time=0.5):
exitError(8)
if velma.waitForEffectorRight() != 0:
exitError(9)
rospy.sleep(0.5)
print "calculating difference between desiread and reached pose..."
T_B_T_diff = PyKDL.diff(T_B_Trd, velma.getTf("B", "Tr"), 1.0)
print T_B_T_diff
if T_B_T_diff.vel.Norm() > 0.05 or T_B_T_diff.rot.Norm() > 0.05:
releaseRight()
(beer_frame, beer_angle) = locateObject("beer")
moveRight(beer_frame.p[0] - 0.5 * math.cos(theta),
beer_frame.p[1] - 0.5 * math.sin(theta),
0.5 * h_puszki + beer_frame.p[2],
beer_angle) #podnosimy reke zeby nie potracic puszki
grabRight(0)
planAndExecute(q_default_position)
exitError(10)
def torque_controller_cb(self, event):
if rospy.is_shutdown() or self.cart_command == None:
return
elapsed_time = rospy.Time.now() - self.cart_command.header.stamp
if elapsed_time.to_sec() > self.timeout:
return
# TODO: Validate msg.header.frame_id
## Cartesian error to zero using a Jacobian transpose controller
x_target = posemath.fromMsg(self.cart_command.pose)
q = self.get_joint_angles_array()
x = baxter_to_kdl_frame(self.arm_interface.endpoint_pose())
xdot = baxter_to_kdl_twist(self.arm_interface.endpoint_velocity())
# Calculate a Cartesian restoring wrench
x_error = PyKDL.diff(x_target, x)
wrench = np.matrix(np.zeros(6)).T
for i in range(len(wrench)):
wrench[i] = -(self.kp[i] * x_error[i] + self.kd[i] * xdot[i])
# Calculate the jacobian
J = self.kinematics.jacobian(q)
# Convert the force into a set of joint torques. tau = J^T * wrench
tau = J.T * wrench
# Populate the joint_torques in baxter API format (dictionary)
joint_torques = dict()
for i, name in enumerate(self.joint_names):
joint_torques[name] = tau[i]
self.arm_interface.set_joint_torques(joint_torques)
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 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 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 calc_rel_pose(source_pose_msg, target_pose_msg):
(source_frame_id, source_frame) = poseStamped_to_kdlFrame(source_pose_msg)
(target_frame_id, target_frame) = poseStamped_to_kdlFrame(target_pose_msg)
if target_frame_id != source_frame_id:
return None
t = kdl.diff(target_frame, source_frame)
return (t[0], t[1], t[2])
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 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 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 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 moveCart(B_T):
print "Zaczynam ruch nadgarstka"
if not velma.moveCartImpRight([B_T], [3.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5):
exitError(16)
if velma.waitForEffectorRight() != 0:
exitError(17)
rospy.sleep(0.5)
print "calculating difference between desiread and reached pose..."
T_B_T_diff = PyKDL.diff(B_T, velma.getTf("B", "Tr"), 1.0)
print T_B_T_diff
if T_B_T_diff.vel.Norm() > 0.05 or T_B_T_diff.rot.Norm() > 0.05:
exitError(10)
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 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 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 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 pi_controller(self, world):
# get current state
x = world.get_object_pose(self._object_name)
xdot = world.get_object_twist(self._object_name)
# calculate error terms
e = kdl.diff(x, self._goal_pose)
edot = self._goal_twist - xdot
# calculate control wrench and apply it
control_law = self._p_gain * e + self._d_gain * edot
# return kdl.Wrench(control_law.vel, control_law.rot)
return kdl.Wrench(control_law.vel,
kdl.Vector()) # TODO: fix rot control
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 resolvedRates(self, currentPose, desiredPose):
# compute pose error (result in kdl.twist format)
poseError = PyKDL.diff(currentPose, desiredPose)
posErrNorm = poseError.vel.Norm()
rotErrNorm = poseError.rot.Norm()
# compute velocity magnitude based on position error norm
if posErrNorm > self.resolvedRatesConfig['tolPos']:
tolPosition = self.resolvedRatesConfig['tolPos']
lambdaVel = self.resolvedRatesConfig['velRatio']
if posErrNorm > (lambdaVel * tolPosition):
velMag = self.resolvedRatesConfig['velMax']
else:
velMax = self.resolvedRatesConfig['velMax']
velMin = self.resolvedRatesConfig['velMin']
velMag = (velMin \
+ (posErrNorm - tolPosition) \
* (velMax-velMin) / (tolPosition * (lambdaVel-1)))
else:
velMag = 0.0
# compute angular velocity based on rotation error norm
if rotErrNorm > self.resolvedRatesConfig['tolRot']:
tolRotation = self.resolvedRatesConfig['tolRot']
lambdaRot = self.resolvedRatesConfig['rotRatio']
if rotErrNorm > (lambdaRot * tolRotation):
angVelMag = self.resolvedRatesConfig['angVelMax']
else:
angVelMax = self.resolvedRatesConfig['angVelMax']
angVelMin = self.resolvedRatesConfig['angVelMin']
angVelMag = (angVelMin \
+ (rotErrNorm - tolRotation) \
* (angVelMax - angVelMin) \
/ (tolRotation * (lambdaRot-1)))
else:
angVelMag = 0.0
# The resolved rates is implemented as Nabil Simaan's notes
# apply both the velocity and angular velocity in the error pose direction
desiredTwist = PyKDL.Twist()
poseError.vel.Normalize() # normalize to have the velocity direction
desiredTwist.vel = poseError.vel * velMag
poseError.rot.Normalize() # normalize to have the ang vel direction
desiredTwist.rot = poseError.rot * angVelMag
# Check whether we have reached our goal
# goalReached = desiredTwist.vel.Norm() <= self.resolvedRatesConfig['velMin'] \
# and desiredTwist.rot.Norm() <= self.resolvedRatesConfig['angVelMin']
# Ignore rotation for calculating goalReached!
goalReached = desiredTwist.vel.Norm(
) <= self.resolvedRatesConfig['velMin']
return desiredTwist, goalReached
def check_ik_result_using_fk(fk_solver, result_joint_state_kdl,
goal_frame_kdl):
fk_frame_kdl = PyKDL.Frame()
fk_solver.JntToCart(result_joint_state_kdl, fk_frame_kdl)
distance_vec = PyKDL.diff(fk_frame_kdl.p, goal_frame_kdl.p)
norm = math.sqrt(PyKDL.dot(distance_vec, distance_vec))
print "distance norm: " + str(norm)
relative_rotation = fk_frame_kdl.M.Inverse() * goal_frame_kdl.M
distance_angle, _ = relative_rotation.GetRotAngle()
print "distance angle: " + str(distance_angle)
goal_pose_reached = (norm < 0.005) and (abs(distance_angle) < 0.1)
return goal_pose_reached
def moveCart(T_B_Trd): # cart
global move_time
t=countTime(T_B_Trd)
print("Moving right wrist to pose defined in world frame...")
if not velma.moveCartImpRight([T_B_Trd], [move_time*t], None, None, None, None, makeWrench([5,5,5], [5,5,5]), start_time=0.01):
exitError(13)
if velma.waitForEffectorRight() != 0:
exitError(14)
rospy.sleep(0.5)
print("Calculating difference between desiread and reached pose...")
T_B_T_diff = PyKDL.diff(T_B_Trd, velma.getTf("B", "Tr"), 1.0)
if T_B_T_diff.vel.Norm() > 0.05 or T_B_T_diff.rot.Norm() > 0.05:
exitError(15)
def resolvedRates(config, currentPose, desiredPose):
# compute pose error (result in kdl.twist format)
poseError = PyKDL.diff(currentPose, desiredPose)
posErrNorm = poseError.vel.Norm()
rotErrNorm = poseError.rot.Norm()
angVelMag = config['angVelMax']
if rotErrNorm < config['tolRot']:
angVelMag = 0.0
elif rotErrNorm < angVelMag:
angVelMag = rotErrNorm
# compute velocity magnitude based on position error norm
if posErrNorm > config['tolPos']:
tolPosition = config['tolPos']
lambdaVel = config['velRatio']
if posErrNorm > (lambdaVel * tolPosition):
velMag = config['velMax']
else:
velMax = config['velMax']
velMin = config['velMin']
velMag = velMin + (posErrNorm - tolPosition) * \
(velMax - velMin)/(tolPosition*(lambdaVel-1))
else:
velMag = 0.0
# compute angular velocity based on rotation error norm
if rotErrNorm > config['tolRot']:
tolRotation = config['tolRot']
lambdaRot = config['rotRatio']
if rotErrNorm > (lambdaRot * tolRotation):
angVelMag = config['angVelMax']
else:
angVelMax = config['angVelMax']
angVelMin = config['angVelMin']
angVelMag = angVelMin + (rotErrNorm - tolRotation) * \
(angVelMax - angVelMin)/(tolRotation*(lambdaRot-1))
else:
angVelMag = 0.0
# The resolved rates is implemented as Nabil Simaan's notes
# apply both the velocity and angular velocity in the error pose direction
desiredTwist = PyKDL.Twist()
poseError.vel.Normalize() # normalize to have the velocity direction
desiredTwist.vel = poseError.vel * velMag
poseError.rot.Normalize() # normalize to have the ang vel direction
desiredTwist.rot = poseError.rot * angVelMag
return desiredTwist
def get_err_twist(self, goal_frame=None, dT=0.01):
""" Get the desirable twist for given error.
"""
#_cur_frame = self.get_current_frame()
if self.btn_state_2 == 1:
b_T_saw0 = pm.toMatrix(self.cur_frame)
b_T_vr0 = pm.toMatrix(self._frame2)
self.vr0_T_saw0 = np.matmul(inv(b_T_vr0),
b_T_saw0) # numpy 4x4 array
# else:
# self.vr0_T_saw0 = np.identity(4)
self._frame2 = pm.fromMatrix(
np.matmul(self.vr0_T_saw0, pm.toMatrix(self._frame2)))
return PyKDL.diff(self.cur_frame, self._frame2, dT)
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 move_end_effector_to_cartesian_position_and_angle_and_wrench(
which_hand, angleX, angleY, angleZ, x, y, z, imp_list, imp_times,
max_wrench, path_tool_tolerance, time):
global velma
T_hand = PyKDL.Frame(PyKDL.Rotation.RPY(angleX, angleY, angleZ),
PyKDL.Vector(x, y, z))
if which_hand == RIGHT_HAND:
if not velma.moveCartImpRight([T_hand],
time,
None,
None,
imp_list,
imp_times,
max_wrench,
start_time=0.5,
path_tol=path_tool_tolerance):
exitError(13)
error = velma.waitForEffectorRight()
if error != 0:
if error == PATH_TOLERANCE_VIOLATED_ERROR:
return PATH_TOLERANCE_VIOLATED_ERROR
elif which_hand == LEFT_HAND:
if not velma.moveCartImpLeft([T_hand], [3.0],
None,
None,
imp_list,
imp_times,
max_wrench,
start_time=0.5,
path_tol=path_tool_tolerance):
exitError(13)
error = velma.waitForEffectorLeft()
if error != 0:
if error == PATH_TOLERANCE_VIOLATED_ERROR:
return PATH_TOLERANCE_VIOLATED_ERROR
else:
print "Error - none hand has been specified!"
rospy.sleep(0.2)
# Check precision
if which_hand == RIGHT_HAND:
T_hand_diff = PyKDL.diff(T_hand, velma.getTf("B", "Tr"), 1.0)
if T_hand_diff.vel.Norm() > 0.05 or T_hand_diff.rot.Norm() > 0.05:
print "Precision for right hand is not perfect!"
elif which_hand == LEFT_HAND:
print "d 1.13 - check precision for the left hand - later"
def compute_twist(T0, T1):
"""
Compute the twist between two homogeneous transformations: `twist = T1 - T0`
Parameters
----------
T0: array_like
Initial homogeneous transformation
T1: array_like
Final homogeneous transformation
"""
F0 = posemath.fromMatrix(T0)
F1 = posemath.fromMatrix(T1)
kdl_twist = PyKDL.diff(F0, F1)
twist = np.zeros(6)
twist[:3] = [kdl_twist.vel.x(), kdl_twist.vel.y(), kdl_twist.vel.z()]
twist[3:] = [kdl_twist.rot.x(), kdl_twist.rot.y(), kdl_twist.rot.z()]
return twist
def resolvedRates(self):
# get current and desired robot pose (desired is the top of queue)
currentPose = self.robot.get_current_position()
desiredPose = self.trajectory[0]
# compute pose error (result in kdl.twist format)
poseError = PyKDL.diff(currentPose, desiredPose)
posErrNorm = poseError.vel.Norm()
rotErrNorm = poseError.rot.Norm()
# compute velocity magnitude based on position error norm
if posErrNorm > self.resolvedRatesConfig['tolPos']:
tolPosition = self.resolvedRatesConfig['tolPos']
lambdaVel = self.resolvedRatesConfig['velRatio']
if posErrNorm > (lambdaVel * tolPosition):
velMag = self.resolvedRatesConfig['velMax']
else:
velMax = self.resolvedRatesConfig['velMax']
velMin = self.resolvedRatesConfig['velMin']
velMag = velMin + \
(velMax - velMin)/(tolPosition*(lambdaVel-1))
else:
velMag = 0.0
# compute angular velocity based on rotation error norm
if rotErrNorm > self.resolvedRatesConfig['tolRot']:
tolRotation = self.resolvedRatesConfig['tolRot']
lambdaRot = self.resolvedRatesConfig['rotRatio']
if posErrNorm > (lambdaRot * tolRotation):
angVelMag = self.resolvedRatesConfig['angVelMax']
else:
angVelMax = self.resolvedRatesConfig['angVelMax']
angVelMin = self.resolvedRatesConfig['angVelMin']
angVelMag = angVelMin + \
(angVelMax - angVelMin)/(tolRotation*(lambdaRot-1))
else:
angVelMag = 0.0
# The resolved rates is implemented as Nabil Simaan's notes
# apply both the velocity and angular velocity in the error pose direction
sys_dt = self.resolvedRatesConfig['dt']
desiredTwist = PyKDL.Twist()
poseError.vel.Normalize() # normalize to have the velocity direction
desiredTwist.vel = poseError.vel * velMag * sys_dt
poseError.rot.Normalize() # normalize to have the ang vel direction
desiredTwist.rot = poseError.rot * angVelMag * sys_dt
return desiredTwist
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 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 try_move(self, frame):
#initialize joint values for search to current angles
q_out = kdl.JntArray(self.jnt_pos)
delta_q = kdl.JntArray(self.jnt_pos.rows())
maxiter = 300
eps = 0.002
f = Frame()
searching = True
iter = 0
while (searching):
iter += 1
#forward kin of the current joints
self.fk_solver.JntToCart(q_out, f)
#distance in 6DOF between goal and current frame
delta_twist = kdl.diff(f, frame)
#velocity inv kinematics
self.ikv_solver.CartToJnt(q_out, delta_twist, delta_q)
#Add delta angle
Add(q_out, delta_q, q_out)
#Algorithm has converged:
print "Delta_twist:"
print delta_twist
if ((abs(delta_twist.vel[0]) < eps)
and (abs(delta_twist.vel[1]) < eps)
and (abs(delta_twist.vel[2]) < eps)):
print("found a solution")
searching = False
self.jnt_pos = q_out
return (True)
if (iter > maxiter):
print "IK: Did not converge"
return False
def get_des_twist(self, dT=0.01):
""" Get desired twist @ time t, by computing differential of traj_frame @ t=T-1 and t=T
"""
#_cur_frame = self.get_current_frame()
#return PyKDL.diff(self.prev_frame, _cur_frame, dT)
#
#
if self.btn_state_2 == 1:
b_T_saw0 = pm.toMatrix(self.cur_frame)
b_T_vr0 = pm.toMatrix(self._frame2)
self.vr0_T_saw0 = np.matmul(inv(b_T_vr0),
b_T_saw0) # numpy 4x4 array
# else:
# self.vr0_T_saw0 = np.identity(4)
self._frame2 = pm.fromMatrix(
np.matmul(self.vr0_T_saw0, pm.toMatrix(self._frame2)))
self._frame1 = pm.fromMatrix(
np.matmul(self.vr0_T_saw0, pm.toMatrix(self._frame1)))
return PyKDL.diff(self._frame1, self._frame2, dT)
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_O = self.openrave.T_World_Br.Inverse() * T_World_E * self.T_E_O
diff = PyKDL.diff(self.T_B_O_nearHole, T_B_O)
if diff.vel.Norm() > 0.02 or diff.rot.Norm() > 10.0/180.0*math.pi:
return False
angle1 = 0.0
for T_B_W, angle in self.key_traj1_T_B_W:
init_js = self.openrave.getRobotConfigurationRos()
traj = self.velma_solvers.getCartImpWristTraj(init_js, T_B_W)
if traj == None:
break
angle1 = angle
qar = {}
for qi in range(len(traj[-1])):
qar["right_arm_"+str(qi)+"_joint"] = traj[-1][qi]
self.openrave.updateRobotConfigurationRos(qar)
self.openrave.robot_rave.SetDOFValues(q, self.dof_indices)
angle2 = 0.0
for T_B_W, angle in self.key_traj2_T_B_W:
init_js = self.openrave.getRobotConfigurationRos()
traj = self.velma_solvers.getCartImpWristTraj(init_js, T_B_W)
if traj == None:
break
angle2 = angle
qar = {}
for qi in range(len(traj[-1])):
qar["right_arm_"+str(qi)+"_joint"] = traj[-1][qi]
self.openrave.updateRobotConfigurationRos(qar)
if abs(angle1-angle2) > 190.0/180.0*math.pi:
return True
else:
return False
def moveToInteractiveCursor(velma):
T_Wo_test = velma.getTf("Wo", "example_frame")
if not velma.moveCartImpRightCurrentPos(start_time=0.2):
exitError(8)
if velma.waitForEffectorRight() != 0:
exitError(9)
if not velma.moveCartImpRight([T_Wo_test], [3.0],
None,
None,
None,
None,
PyKDL.Wrench(PyKDL.Vector(5, 5, 5),
PyKDL.Vector(5, 5, 5)),
start_time=0.5):
exitError(8)
if velma.waitForEffectorRight() != 0:
exitError(9)
rospy.sleep(0.5)
T_B_T_diff = PyKDL.diff(T_Wo_test, velma.getTf("B", "Tr"), 1.0)
if T_B_T_diff.vel.Norm() > 0.05 or T_B_T_diff.rot.Norm() > 0.05:
exitError(10)
def torque_controller_cb(self, event):
if rospy.is_shutdown() or None in [
self.cart_command, self.endpoint_state, self.joint_states
]:
return
# TODO: Validate msg.header.frame_id
## Cartesian error to zero using a Jacobian transpose controller
x_target = posemath.fromMsg(self.cart_command.pose)
q, qd, eff = self.kinematics.extract_joint_state(self.joint_states)
x = posemath.fromMsg(self.endpoint_state.pose)
xdot = TwistMsgToKDL(self.endpoint_state.twist)
# Calculate a Cartesian restoring wrench
x_error = PyKDL.diff(x_target, x)
wrench = np.matrix(np.zeros(6)).T
for i in range(len(wrench)):
wrench[i] = -(self.kp[i] * x_error[i] + self.kd[i] * xdot[i])
# Calculate the jacobian
J = self.kinematics.jacobian(q)
# Convert the force into a set of joint torques. tau = J^T * wrench
tau = J.T * wrench
# Publish the joint_torques
for i, name in enumerate(self.joint_names):
self.torque_pub[name].publish(tau[i])
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)
print "Switch to cart_imp mode (no trajectory)..."
if not velma.moveCartImpRightCurrentPos(start_time=0.2):
exitError(8)
if velma.waitForEffectorRight() != 0:
exitError(9)
rospy.sleep(0.5)
print "Moving right wrist to pose defined in world frame..."
T_B_Trd = PyKDL.Frame(PyKDL.Rotation.RPY(0, -0.1, -0.2), PyKDL.Vector( 0.25 , -0.25 , 1.3 ))
if not velma.moveCartImpRight([T_B_Trd], [3.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5):
exitError(8)
if velma.waitForEffectorRight() != 0:
exitError(9)
rospy.sleep(0.5)
print "calculating difference between desiread and reached pose..."
T_B_T_diff = PyKDL.diff(T_B_Trd, velma.getTf("B", "Tr"), 1.0)
print T_B_T_diff
if T_B_T_diff.vel.Norm() > 0.1 or T_B_T_diff.rot.Norm() > 0.1:
exitError(10)
js = velma.getLastJointState()
print js
exitError(0)
def spin(self):
rospack = rospkg.RosPack()
model = "5dof"
# model = "6dof"
# model = "two_arms"
if model == "6dof":
urdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator_6dof.urdf'
srdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator_6dof.srdf'
elif model == "5dof":
urdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator.urdf'
srdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator.srdf'
elif model == "two_arms":
urdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_two_arms.urdf'
srdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_two_arms.srdf'
# TEST: line - line distance
if False:
while not rospy.is_shutdown():
pt = PyKDL.Vector(random.uniform(-1, 1), random.uniform(-1, 1), 0)
line = (PyKDL.Vector(random.uniform(-1, 1),random.uniform(-1, 1),0), PyKDL.Vector(random.uniform(-1, 1),random.uniform(-1, 1),0))
line2 = (PyKDL.Vector(random.uniform(-1, 1),random.uniform(-1, 1),0), PyKDL.Vector(random.uniform(-1, 1),random.uniform(-1, 1),0))
dist, p1, p2 = self.distanceLines(line, line2)
m_id = 0
m_id = self.pub_marker.publishVectorMarker(line[0], line[1], m_id, 0, 1, 0, frame='world', namespace='default', scale=0.02)
m_id = self.pub_marker.publishVectorMarker(line2[0], line2[1], m_id, 1, 0, 0, frame='world', namespace='default', scale=0.02)
m_id = self.pub_marker.publishVectorMarker(p1, p2, m_id, 1, 1, 1, frame='world', namespace='default', scale=0.02)
print line, line2, dist
raw_input(".")
exit(0)
# TEST: point - point distance
if False:
while not rospy.is_shutdown():
pt1 = PyKDL.Vector(random.uniform(-1, 1), random.uniform(-1, 1), 0)
pt2 = PyKDL.Vector(random.uniform(-1, 1), random.uniform(-1, 1), 0)
dist, p1, p2 = self.distancePoints(pt1, pt2)
m_id = 0
m_id = self.pub_marker.publishSinglePointMarker(p1, m_id, r=0, g=1, b=0, a=1, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.1, 0.1, 0.1), T=None)
m_id = self.pub_marker.publishSinglePointMarker(p2, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.1, 0.1, 0.1), T=None)
m_id = self.pub_marker.publishVectorMarker(p1, p2, m_id, 1, 0, 0, frame='world', namespace='default', scale=0.02)
print pt1, pt2, dist
raw_input(".")
exit(0)
col = collision_model.CollisionModel()
col.readUrdfSrdf(urdf_path, srdf_path)
if model == "6dof":
self.joint_names = ["0_joint", "1_joint", "2_joint", "3_joint", "4_joint", "5_joint"]
effector_name = 'effector'
elif model == "5dof":
self.joint_names = ["0_joint", "1_joint", "2_joint", "3_joint", "4_joint"]
effector_name = 'effector'
elif model == "two_arms":
self.joint_names = ["torso_0_joint", "left_0_joint", "left_1_joint", "left_2_joint", "left_3_joint", "right_0_joint", "right_1_joint", "right_2_joint", "right_3_joint"]
effector_name = 'left_effector'
self.q = np.zeros( len(self.joint_names) )
test_cases = [
(1.1, -2.3, 1.5, 1.5, 1.5),
(-1.1, 2.3, -1.5, -1.5, -1.5),
(0.0, 0.0, 1.5, 1.5, 1.5),
(0.0, 0.0, -1.5, -1.5, -1.5),
(0.2, 0.4, 1.5, 1.5, 1.5),
(-0.2, -0.4, -1.5, -1.5, -1.5),
]
# self.q[0] = 1.1
# self.q[1] = -2.3
# self.q[2] = 1.5
# self.q[3] = 1.5
# self.q[4] = 1.5
# self.q = np.array(test_cases[4])
solver = fk_ik.FkIkSolver(self.joint_names, [], None)
self.publishJointState()
# rospy.sleep(1)
# exit(0)
obst = []
obj = collision_model.CollisionModel.Collision()
obj.type = "capsule"
obj.T_L_O = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi), PyKDL.Vector(1.5, 0.7, 0))
obj.radius = 0.1
obj.length = 0.4
obst.append( obj )
obj = collision_model.CollisionModel.Collision()
obj.type = "capsule"
obj.T_L_O = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi), PyKDL.Vector(1.2, -0.9, 0))
obj.radius = 0.1
obj.length = 0.4
obst.append( obj )
obj = collision_model.CollisionModel.Collision()
obj.type = "sphere"
obj.T_L_O = PyKDL.Frame(PyKDL.Vector(1, 0, 0))
obj.radius = 0.05
obst.append( obj )
# rospy.sleep(1)
# T_B_E = solver.calculateFk("base", "effector", self.q)
# print T_B_E
r_HAND_targets = [
PyKDL.Frame(PyKDL.Vector(0.1,1.0,0.0)),
PyKDL.Frame(PyKDL.Vector(0.1,1.7,0.0)),
# PyKDL.Frame(PyKDL.Rotation.RotY(90.0/180.0*math.pi), PyKDL.Vector(0.2,-0.5,0.0)),
]
target_idx = 0
r_HAND_target = r_HAND_targets[target_idx]
target_idx += 1
r_HAND_target = PyKDL.Frame(PyKDL.Vector(0.5,0.5,0))
last_time = rospy.Time.now()
counter = 10000
while not rospy.is_shutdown():
if counter > 800:
if model == "two_arms":
r_HAND_target = PyKDL.Frame(PyKDL.Rotation.RotZ(random.uniform(-math.pi, math.pi)), PyKDL.Vector(random.uniform(-1,0), random.uniform(0,1.8), 0))
else:
r_HAND_target = PyKDL.Frame(PyKDL.Rotation.RotZ(random.uniform(-math.pi, math.pi)), PyKDL.Vector(random.uniform(0,2), random.uniform(-1,1), 0))
# r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,-0.98100002989,0.194007580664), PyKDL.Vector(-0.129108034334,0.518606130706,0.0))
# r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.470814280381,0.882232346601), PyKDL.Vector(0.676567476122,0.0206561816531,0.0))
#r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.50451570265,0.863402516663), PyKDL.Vector(0.252380653828,0.923309935287,0.0))
#self.q = np.array( [-0.29968745 0.66939973 -2.49850991 1.87533697 2.63546305] )
#r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.704294768084,0.70990765572), PyKDL.Vector(0.334245569765,1.82368612057,0.0))
#self.q = np.array( [ 0.33203731 0.071835 -2.46646112 1.14339024 1.97684146] )
# r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.924467039084,-0.381261975087), PyKDL.Vector(0.261697539135,0.97235224304,0.0))
#r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.894763681298,0.446539980999), PyKDL.Vector(0.354981453046,0.604598917063,0.0))
#self.q = np.array( [-0.89640518 0.44336642 1.96125279 -1.66533209 -2.19189403] )
qt = r_HAND_target.M.GetQuaternion()
pt = r_HAND_target.p
print "r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s,%s,%s,%s), PyKDL.Vector(%s,%s,%s))"%(qt[0],qt[1],qt[2],qt[3],pt[0],pt[1],pt[2])
print "self.q = np.array(", self.q, ")"
counter = 0
counter += 1
time_elapsed = rospy.Time.now() - last_time
J_JLC = np.matrix(numpy.zeros( (len(self.q), len(self.q)) ))
delta_V_JLC = np.empty(len(self.q))
for q_idx in range(len(self.q)):
if self.q[q_idx] < solver.lim_lower_soft[q_idx]:
delta_V_JLC[q_idx] = self.q[q_idx] - solver.lim_lower_soft[q_idx]
J_JLC[q_idx,q_idx] = min(1.0, 10*abs(self.q[q_idx] - solver.lim_lower_soft[q_idx]) / abs(solver.lim_lower[q_idx] - solver.lim_lower_soft[q_idx]))
elif self.q[q_idx] > solver.lim_upper_soft[q_idx]:
delta_V_JLC[q_idx] = self.q[q_idx] - solver.lim_upper_soft[q_idx]
J_JLC[q_idx,q_idx] = min(1.0, 10*abs(self.q[q_idx] - solver.lim_upper_soft[q_idx]) / abs(solver.lim_upper[q_idx] - solver.lim_upper_soft[q_idx]))
else:
delta_V_JLC[q_idx] = 0.0
J_JLC[q_idx,q_idx] = 0.0
J_JLC_inv = J_JLC.transpose()#np.linalg.pinv(J_JLC)
N_JLC = np.matrix(np.identity(len(self.q))) - (J_JLC_inv * J_JLC)
N_JLC_inv = np.linalg.pinv(N_JLC)
v_max_JLC = 20.0/180.0*math.pi
kp_JLC = 10.0
dx_JLC_des = kp_JLC * delta_V_JLC
# min(1.0, v_max_JLC/np.linalg.norm(dx_JLC_des))
if v_max_JLC > np.linalg.norm(dx_JLC_des):
vv_JLC = 1.0
else:
vv_JLC = v_max_JLC/np.linalg.norm(dx_JLC_des)
dx_JLC_ref = - vv_JLC * dx_JLC_des
J_r_HAND = solver.getJacobian('base', effector_name, self.q, base_end=False)
J_r_HAND_inv = np.linalg.pinv(J_r_HAND)
T_B_E = solver.calculateFk('base', effector_name, self.q)
r_HAND_current = T_B_E
r_HAND_diff = PyKDL.diff(r_HAND_current, r_HAND_target)
delta_V_HAND = np.empty(6)
delta_V_HAND[0] = r_HAND_diff.vel[0]
delta_V_HAND[1] = r_HAND_diff.vel[1]
delta_V_HAND[2] = r_HAND_diff.vel[2]
delta_V_HAND[3] = r_HAND_diff.rot[0]
delta_V_HAND[4] = r_HAND_diff.rot[1]
delta_V_HAND[5] = r_HAND_diff.rot[2]
v_max_HAND = 4.0
kp_HAND = 10.0
dx_HAND_des = kp_HAND * delta_V_HAND
if v_max_HAND > np.linalg.norm(dx_HAND_des):
vv_HAND = 1.0
else:
vv_HAND = v_max_HAND/np.linalg.norm(dx_HAND_des)
dx_r_HAND_ref = vv_HAND * dx_HAND_des
links_fk = {}
for link in col.links:
links_fk[link.name] = solver.calculateFk('base', link.name, self.q)
activation_dist = 0.05
link_collision_map = {}
if True:
# self collision
total_contacts = 0
for link1_name, link2_name in col.collision_pairs:
link1 = col.link_map[link1_name]
T_B_L1 = links_fk[link1_name]
link2 = col.link_map[link2_name]
T_B_L2 = links_fk[link2_name]
for col1 in link1.col:
for col2 in link2.col:
T_B_C1 = T_B_L1 * col1.T_L_O
T_B_C2 = T_B_L2 * col2.T_L_O
c_dist = (T_B_C1 * PyKDL.Vector() - T_B_C2 * PyKDL.Vector()).Norm()
if col1.type == "capsule":
c_dist -= col1.radius + col1.length/2
elif col1.type == "sphere":
c_dist -= col1.radius
if col2.type == "capsule":
c_dist -= col2.radius + col2.length/2
elif col2.type == "sphere":
c_dist -= col2.radius
if c_dist > activation_dist:
continue
dist = None
if col1.type == "capsule" and col2.type == "capsule":
line1 = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0))
line2 = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0))
dist, p1_B, p2_B = self.distanceLines(line1, line2)
elif col1.type == "capsule" and col2.type == "sphere":
line = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0))
pt = T_B_C2 * PyKDL.Vector()
dist, p1_B, p2_B = self.distanceLinePoint(line, pt)
elif col1.type == "sphere" and col2.type == "capsule":
pt = T_B_C1 * PyKDL.Vector()
line = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0))
dist, p1_B, p2_B = self.distancePointLine(pt, line)
elif col1.type == "sphere" and col2.type == "sphere":
dist, p1_B, p2_B = self.distancePoints(T_B_C1 * PyKDL.Vector(), T_B_C2 * PyKDL.Vector())
else:
print "ERROR: unknown collision type:", col1.type, col2.type
exit(0)
if dist != None:
# print "dist", dist, link1_name, link2_name, col1.type, col2.type
dist -= col1.radius + col2.radius
v = p2_B - p1_B
v.Normalize()
n1_B = v
n2_B = -v
p1_B += n1_B * col1.radius
p2_B += n2_B * col2.radius
if dist < activation_dist:
if not (link1_name, link2_name) in link_collision_map:
link_collision_map[(link1_name, link2_name)] = []
link_collision_map[(link1_name, link2_name)].append( (p1_B, p2_B, dist, n1_B, n2_B) )
# environment collisions
for link in col.links:
if link.col == None:
continue
link1_name = link.name
T_B_L1 = links_fk[link1_name]
T_B_L2 = links_fk["base"]
for col1 in link.col:
for col2 in obst:
T_B_C1 = T_B_L1 * col1.T_L_O
T_B_C2 = T_B_L2 * col2.T_L_O
dist = None
if col1.type == "capsule" and col2.type == "capsule":
line1 = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0))
line2 = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0))
dist, p1_B, p2_B = self.distanceLines(line1, line2)
elif col1.type == "capsule" and col2.type == "sphere":
line = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0))
pt = T_B_C2 * PyKDL.Vector()
dist, p1_B, p2_B = self.distanceLinePoint(line, pt)
elif col1.type == "sphere" and col2.type == "capsule":
pt = T_B_C1 * PyKDL.Vector()
line = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0))
dist, p1_B, p2_B = self.distancePointLine(pt, line)
elif col1.type == "sphere" and col2.type == "sphere":
dist, p1_B, p2_B = self.distancePoints(T_B_C1 * PyKDL.Vector(), T_B_C2 * PyKDL.Vector())
# print "a:",dist, p1_B, p2_B
else:
print "ERROR: unknown collision type:", col1.type, col2.type
exit(0)
if dist != None:
dist -= col1.radius + col2.radius
v = p2_B - p1_B
v.Normalize()
n1_B = v
n2_B = -v
p1_B += n1_B * col1.radius
p2_B += n2_B * col2.radius
# if col1.type == "sphere" and col2.type == "sphere":
# print "b:",dist, p1_B, p2_B
if dist < activation_dist:
if not (link1_name, "base") in link_collision_map:
link_collision_map[(link1_name, "base")] = []
link_collision_map[(link1_name, "base")].append( (p1_B, p2_B, dist, n1_B, n2_B) )
omega_col = np.matrix(np.zeros( (len(self.q),1) ))
Ncol = np.matrix(np.identity(len(self.q)))
m_id = 0
for link1_name, link2_name in link_collision_map:
for (p1_B, p2_B, dist, n1_B, n2_B) in link_collision_map[ (link1_name, link2_name) ]:
if dist > 0.0:
m_id = self.pub_marker.publishVectorMarker(p1_B, p2_B, m_id, 1, 1, 1, frame='base', namespace='default', scale=0.02)
else:
m_id = self.pub_marker.publishVectorMarker(p1_B, p2_B, m_id, 1, 0, 0, frame='base', namespace='default', scale=0.02)
T_B_L1 = links_fk[link1_name]
T_L1_B = T_B_L1.Inverse()
T_B_L2 = links_fk[link2_name]
T_L2_B = T_B_L2.Inverse()
p1_L1 = T_L1_B * p1_B
p2_L2 = T_L2_B * p2_B
n1_L1 = PyKDL.Frame(T_L1_B.M) * n1_B
n2_L2 = PyKDL.Frame(T_L2_B.M) * n2_B
# print p1_L1, p1_L1+n1_L1*0.2
# print p2_L2, p2_L2+n2_L2*0.2
# m_id = self.pub_marker.publishVectorMarker(p1_L1, p1_L1+n1_L1*0.2, m_id, 0, 1, 0, frame=link1_name, namespace='default', scale=0.02)
# m_id = self.pub_marker.publishVectorMarker(T_B_L2*p2_L2, T_B_L2*(p2_L2+n2_L2*0.2), m_id, 0, 0, 1, frame="base", namespace='default', scale=0.02)
jac1 = PyKDL.Jacobian(len(self.q))
jac2 = PyKDL.Jacobian(len(self.q))
common_link_name = solver.getJacobiansForPairX(jac1, jac2, link1_name, p1_L1, link2_name, p2_L2, self.q, None)
# T_B_Lc = links_fk[common_link_name]
# jac1.changeBase(T_B_Lc.M)
# jac2.changeBase(T_B_Lc.M)
# T_Lc_L1 = T_B_Lc.Inverse() * T_B_L1
# T_Lc_L2 = T_B_Lc.Inverse() * T_B_L2
# n1_Lc = PyKDL.Frame(T_Lc_L1.M) * n1_L1
# n2_Lc = PyKDL.Frame(T_Lc_L2.M) * n2_L2
# print link1_name, link2_name
# print "jac1"
# print jac1
# print "jac2"
# print jac2
# repulsive velocity
V_max = 20.0
dist = max(dist, 0.0)
depth = (activation_dist - dist)
# Vrep = V_max * depth * depth / (activation_dist * activation_dist)
Vrep = V_max * depth / activation_dist
# Vrep = -max(Vrep, 0.01)
Vrep = -Vrep
# # the mapping between motions along contact normal and the Cartesian coordinates
e1 = n1_L1
e2 = n2_L2
Jd1 = np.matrix([e1[0], e1[1], e1[2]])
Jd2 = np.matrix([e2[0], e2[1], e2[2]])
# rewrite the linear part of the jacobian
jac1_mx = np.matrix(np.zeros( (3, len(self.q)) ))
jac2_mx = np.matrix(np.zeros( (3, len(self.q)) ))
for q_idx in range(len(self.q)):
col1 = jac1.getColumn(q_idx)
col2 = jac2.getColumn(q_idx)
for row_idx in range(3):
jac1_mx[row_idx, q_idx] = col1[row_idx]
jac2_mx[row_idx, q_idx] = col2[row_idx]
Jcol1 = Jd1 * jac1_mx
Jcol2 = Jd2 * jac2_mx
Jcol = np.matrix(np.zeros( (2, len(self.q)) ))
for q_idx in range(len(self.q)):
Jcol[0, q_idx] = Jcol1[0, q_idx]
Jcol[1, q_idx] = Jcol2[0, q_idx]
# Jcol = Jcol * (Ncol * N_JLC)
# TODO: is the transposition ok?
Jcol_pinv = np.linalg.pinv(Jcol)
# Jcol_pinv = Jcol.transpose()
activation = min(1.0, 2.0*depth/activation_dist)
a_des = np.matrix(np.zeros( (len(self.q),len(self.q)) ))
a_des[0,0] = a_des[1,1] = activation
U, S, V = numpy.linalg.svd(Jcol, full_matrices=True, compute_uv=True)
# print "activation", activation
# print "Jcol"
# print Jcol
# raw_input(".")
if rospy.is_shutdown():
exit(0)
# print "V"
# print V
# print "S"
# print S
# Ncol12 = np.matrix(np.identity(len(self.q))) - Jcol.transpose() * (Jcol_pinv).transpose()
Ncol12 = np.matrix(np.identity(len(self.q))) - (V * a_des * V.transpose())
Ncol = Ncol * Ncol12
# d_omega = Jcol_prec_inv * np.matrix([Vrep, Vrep]).transpose()
d_omega = Jcol_pinv * np.matrix([Vrep, Vrep]).transpose()
omega_col += d_omega
# print "omega_col", omega_col
# print dx_HAND_ref
# omega_r_HAND = (J_r_HAND_inv * np.matrix(dx_r_HAND_ref).transpose())
self.pub_marker.eraseMarkers(m_id, 10, frame_id='base', namespace='default')
Ncol_inv = np.linalg.pinv(Ncol)
J_r_HAND_prec = J_r_HAND * (Ncol * N_JLC)
J_r_HAND_prec_inv = np.linalg.pinv(J_r_HAND_prec)
omega = J_JLC_inv * np.matrix(dx_JLC_ref).transpose() + N_JLC.transpose() * (omega_col + (Ncol.transpose() * J_r_HAND_inv) * np.matrix(dx_r_HAND_ref).transpose())
# omega = J_JLC_inv * np.matrix(dx_JLC_ref).transpose() + N_JLC_inv * (omega_col + (Ncol_inv * J_r_HAND_prec_inv) * np.matrix(dx_r_HAND_ref).transpose())
# omega = J_JLC_inv * np.matrix(dx_JLC_ref).transpose() + np.linalg.pinv(N_JLC) * omega_col
# omega = omega_col
omega_vector = np.empty(len(self.q))
for q_idx in range(len(self.q)):
omega_vector[q_idx] = omega[q_idx][0]
max_norm = 0.5
omega_norm = np.linalg.norm(omega_vector)
if omega_norm > max_norm:
omega_vector *= max_norm / omega_norm
self.q += omega_vector * 0.02
if time_elapsed.to_sec() > 0.05:
# print self.q
m_id = 0
m_id = self.publishRobotModelVis(m_id, col, links_fk)
m_id = self.publishObstaclesVis(m_id, obst)
last_time = rospy.Time.now()
self.publishJointState()
self.publishTransform(r_HAND_target, "hand_dest")
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 spin(self):
# locate all markers on the object
if False:
max_angle = 60.0/180.0*math.pi
min_z = math.cos(max_angle)
print "min_z: %s"%(min_z)
# big_box
# marker_list = [18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31]
# small_box
# marker_list = [32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45]
# table
# marker_list = [3,4,5,6]
# c-beam
marker_list = [46, 47, 48, 49, 50, 51, 52, 53, 54, 55]
mtrans_matrix = []
mtrans_weights_ori = []
mtrans_weights_pos = []
for i in range(0, len(marker_list)):
mtrans_matrix.append([])
mtrans_weights_ori.append([])
mtrans_weights_pos.append([])
for j in range(0, len(marker_list)):
if i > j:
mtrans_matrix[i].append([])
mtrans_weights_ori[i].append([])
mtrans_weights_pos[i].append([])
else:
mtrans_matrix[i].append(None)
mtrans_weights_ori[i].append(None)
mtrans_weights_pos[i].append(None)
marker_lock = Lock()
self.visible_markers = []
def markerCallback(data):
marker_lock.acquire()
self.visible_markers = []
for m in data.markers:
if m.id in marker_list:
self.visible_markers.append( (m.id, pm.fromMsg(m.pose.pose)) )
marker_lock.release()
rospy.Subscriber('/ar_pose_marker', AlvarMarkers, markerCallback)
while not rospy.is_shutdown():
marker_lock.acquire()
visible_markers_local = copy.copy(self.visible_markers)
marker_lock.release()
accepted_markers = []
accepted_markers_id = []
accepted_markers_weights_pos = []
accepted_markers_weights_ori = []
for m in visible_markers_local:
n = PyKDL.Frame(m[1].M) * PyKDL.Vector(0,0,-1)
if n.z() > min_z:
accepted_markers.append( m )
accepted_markers_id.append( m[0] )
# n.z() is in range (min_z, 1.0), min_z is the best for orientation and 1.0 i best for position
weight_pos = (n.z() - min_z)/(1.0-min_z)
if weight_pos > 1.0 or weight_pos < 0.0:
print "error: weight==%s"%(weight_pos)
accepted_markers_weights_pos.append(weight_pos)
accepted_markers_weights_ori.append(1.0-weight_pos)
print "visible: %s accepted markers: %s"%(len(visible_markers_local), accepted_markers_id)
for i in range(0, len(accepted_markers)):
for j in range(i+1, len(accepted_markers)):
if accepted_markers[i][0] > accepted_markers[j][0]:
idx1 = i
idx2 = j
else:
idx1 = j
idx2 = i
# print " %s with %s"%(accepted_markers[idx1][0], accepted_markers[idx2][0])
T_C_M1 = accepted_markers[idx1][1]
T_C_M2 = accepted_markers[idx2][1]
T_M1_M2 = T_C_M1.Inverse() * T_C_M2
marker_id1_index = marker_list.index(accepted_markers[idx1][0])
marker_id2_index = marker_list.index(accepted_markers[idx2][0])
mtrans_matrix[marker_id1_index][marker_id2_index].append(T_M1_M2)
mtrans_weights_ori[marker_id1_index][marker_id2_index].append(accepted_markers_weights_ori[idx1] * accepted_markers_weights_ori[idx2])
mtrans_weights_pos[marker_id1_index][marker_id2_index].append(accepted_markers_weights_pos[idx1] * accepted_markers_weights_pos[idx2])
rospy.sleep(0.1)
G = {}
for i in range(0, len(marker_list)):
for j in range(0, len(marker_list)):
if mtrans_matrix[i][j] == None:
pass
# print "%s with %s: None"%(marker_list[i], marker_list[j])
elif len(mtrans_matrix[i][j]) < 10:
print "%s with %s: %s (ignoring)"%(marker_list[i], marker_list[j], len(mtrans_matrix[i][j]))
else:
score, R_M1_M2 = velmautils.meanOrientation(mtrans_matrix[i][j], mtrans_weights_ori[i][j])
# ignore 20% the most distant measures from the mean
distances = []
for measure_idx in range(0, len(mtrans_matrix[i][j])):
diff = PyKDL.diff(R_M1_M2, mtrans_matrix[i][j][measure_idx]).rot.Norm()
distances.append([diff, measure_idx])
distances_sorted = sorted(distances, key=operator.itemgetter(0))
mtrans = []
weights_ori = []
for measure_idx in range(0, int(0.8*len(mtrans_matrix[i][j]))):
mtrans.append(mtrans_matrix[i][j][distances_sorted[measure_idx][1]])
weights_ori.append(mtrans_weights_ori[i][j][distances_sorted[measure_idx][1]])
# score, R_M1_M2 = velmautils.meanOrientation(mtrans, weights_ori)
print "%s with %s: %s, score: %s"%(marker_list[i], marker_list[j], len(mtrans_matrix[i][j]), score)
P_M1_M2 = velmautils.meanPosition(mtrans_matrix[i][j], mtrans_weights_pos[i][j])
print "P_M1_M2 before: %s"%(P_M1_M2)
# ignore 20% the most distant measures from the mean
distances = []
for measure_idx in range(0, len(mtrans_matrix[i][j])):
diff = PyKDL.diff(R_M1_M2, mtrans_matrix[i][j][measure_idx]).vel.Norm()
distances.append([diff, measure_idx])
distances_sorted = sorted(distances, key=operator.itemgetter(0))
mtrans = []
weights_pos = []
for measure_idx in range(0, int(0.8*len(mtrans_matrix[i][j]))):
mtrans.append(mtrans_matrix[i][j][distances_sorted[measure_idx][1]])
weights_pos.append(mtrans_weights_ori[i][j][distances_sorted[measure_idx][1]])
# P_M1_M2 = velmautils.meanPosition(mtrans, weights_pos)
print "P_M1_M2 after: %s"%(P_M1_M2)
mtrans_matrix[i][j] = PyKDL.Frame(copy.deepcopy(R_M1_M2.M), P_M1_M2)
neighbours = G.get(marker_list[i], {})
if score > 0.01:
score = 1000000.0
else:
score = 1.0
neighbours[marker_list[j]] = score
G[marker_list[i]] = neighbours
neighbours = G.get(marker_list[j], {})
neighbours[marker_list[i]] = score
G[marker_list[j]] = neighbours
frames = []
# get the shortest paths weighted by score from optimization
for marker_id in marker_list:
path = dijkstra.shortestPath(G,marker_id,marker_list[0])
print path
T_Ml_Mf = PyKDL.Frame()
for i in range(0, len(path)-1):
if marker_list.index(path[i]) > marker_list.index(path[i+1]):
T_Mp_Mn = mtrans_matrix[marker_list.index(path[i])][marker_list.index(path[i+1])]
T_Ml_Mf = T_Ml_Mf * T_Mp_Mn
else:
T_Mn_Mp = mtrans_matrix[marker_list.index(path[i+1])][marker_list.index(path[i])]
T_Ml_Mf = T_Ml_Mf * T_Mn_Mp.Inverse()
frames.append(copy.deepcopy(T_Ml_Mf.Inverse()))
pub_marker = velmautils.MarkerPublisher()
rospy.sleep(1.0)
m_id = 0
marker_idx = 0
for fr in frames:
q = fr.M.GetQuaternion()
p = fr.p
print "[%s, PyKDL.Frame(PyKDL.Rotation.Quaternion(%s,%s,%s,%s),PyKDL.Vector(%s,%s,%s))],"%(marker_list[marker_idx], q[0], q[1], q[2], q[3], p.x(), p.y(), p.z())
m_id = pub_marker.publishFrameMarker(fr, m_id, scale=0.1, frame='world', namespace='default')
rospy.sleep(0.1)
marker_idx += 1
rospy.sleep(2.0)
exit(0)
else:
pub_marker = velmautils.MarkerPublisher()
rospy.sleep(1.0)
markers2 = [
[46, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.0,1.0),PyKDL.Vector(-0.0,-0.0,-0.0))],
[47, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0161417497092,-9.92379339669e-05,-0.00603105214296,0.999851519216),PyKDL.Vector(-0.0987990318312,-0.000265582834399,0.000544628226204))],
[48, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.706829255712,0.00114672638679,-0.707103949357,0.0198769487466),PyKDL.Vector(0.0427261427894,0.00245374838957,-0.116698579624))],
[49, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.706230029879,-0.00660144281521,-0.70769079068,0.0192174565616),PyKDL.Vector(0.0410352237403,0.000595788239244,-0.0336956438972))],
[50, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0155276433453,0.714580229904,0.00743395758828,0.699341635824),PyKDL.Vector(-0.131991504795,-0.00410930997885,-0.0916799439467))],
[51, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.706160148032,0.0114839861434,-0.707838133957,0.0130820300375),PyKDL.Vector(-0.150701418215,-0.000688426198715,-0.035762545196))],
[52, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.705850422242,0.00260066465892,-0.708005925475,0.022271673869),PyKDL.Vector(-0.150663515172,-0.00196494029406,-0.123356224597))],
[53, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.00630495805624,-0.999979097775,0.000308879730173,0.00139860956497),PyKDL.Vector(-0.0116053782242,0.000352840524022,-0.0267278839783))],
[54, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00853722085061,-0.999853611966,0.00230495916657,0.0146477869582),PyKDL.Vector(-0.0949889134574,0.00131718330416,-0.0165548984986))],
[55, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0208301706621,-0.711731154203,0.0052945617051,0.702123091589),PyKDL.Vector(0.0244779541548,0.000463479220587,-0.0804749548707))],
]
markers = [
[46, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.0,1.0),PyKDL.Vector(-0.0,-0.0,-0.0))],
[47, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00211977224419,0.00133104595822,-0.00433238039783,0.999987482603),PyKDL.Vector(-0.0995553679408,-0.000651966932258,0.000444468330964))],
[48, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.705539374795,0.00129956423061,-0.708394191186,0.0197527628665),PyKDL.Vector(0.0433256677966,0.00212664651843,-0.116482343501))],
[49, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.704707757517,-0.00628228374428,-0.709255128279,0.0174548680028),PyKDL.Vector(0.0412709849952,0.000494665961165,-0.0338667341872))],
[50, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0117276773848,0.706312439798,0.00558379104701,0.707781053891),PyKDL.Vector(-0.131246837996,-0.00469319026484,-0.0943814089463))],
[51, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.710112841604,0.00963407546503,-0.703895468304,0.013345653983),PyKDL.Vector(-0.149984963191,-0.00300459973807,-0.0370193446712))],
[52, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.704691425649,0.00497982940017,-0.709159595229,0.0218601100464),PyKDL.Vector(-0.15277553848,-0.00431480401095,-0.120995842686))],
[53, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.00984066000086,-0.999945658681,0.00299307949816,0.00169781765667),PyKDL.Vector(-0.0132269965227,-0.00110379001368,-0.0274175040768))],
[54, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.00154140261629,0.999633307109,-0.00751679824708,0.0259686953912),PyKDL.Vector(-0.0974091549673,-0.000670004842722,-0.0319416169884))],
[55, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0195903374145,-0.713404165076,0.00273342374532,0.700473585745),PyKDL.Vector(0.0250633176495,-0.00356911439713,-0.0811403242495))],
]
m_id = 0
for m in markers:
print m[0]
print m[1]
m_id = pub_marker.publishFrameMarker(m[1], m_id, scale=0.1, frame='world', namespace='default')
rospy.sleep(0.1)
def spin(self):
if True:
# Create a world object
world = ode.World()
# world.setGravity( (0,0,-3.81) )
world.setGravity( (0,0,0) )
CFM = world.getCFM()
ERP = world.getERP()
print "CFM: %s ERP: %s"%(CFM, ERP)
# world.setCFM(0.001)
# print "CFM: %s ERP: %s"%(CFM, ERP)
m_id = 0
self.pub_marker.eraseMarkers(0,3000, frame_id='world')
#
# Init Openrave
#
parser = OptionParser(description='Openrave Velma interface')
OpenRAVEGlobalArguments.addOptions(parser)
(options, leftargs) = parser.parse_args()
self.env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
# self.openrave_robot = self.env.ReadRobotXMLFile('robots/barretthand_ros.robot.xml')
mimic_joints = [
("right_HandFingerTwoKnuckleOneJoint", "right_HandFingerOneKnuckleOneJoint*1.0", "|right_HandFingerOneKnuckleOneJoint 1.0", ""),
("right_HandFingerOneKnuckleThreeJoint", "right_HandFingerOneKnuckleTwoJoint*0.33333", "|right_HandFingerOneKnuckleTwoJoint 0.33333", ""),
("right_HandFingerTwoKnuckleThreeJoint", "right_HandFingerTwoKnuckleTwoJoint*0.33333", "|right_HandFingerTwoKnuckleTwoJoint 0.33333", ""),
("right_HandFingerThreeKnuckleThreeJoint", "right_HandFingerThreeKnuckleTwoJoint*0.33333", "|right_HandFingerThreeKnuckleTwoJoint 0.33333", ""),
]
rospack = rospkg.RosPack()
self.urdf_module = RaveCreateModule(self.env, 'urdf')
xacro_uri = rospack.get_path('barrett_hand_defs') + '/robots/barrett_hand.urdf.xml'
urdf_uri = '/tmp/barrett_hand.urdf'
srdf_uri = rospack.get_path('barrett_hand_defs') + '/robots/barrett_hand.srdf'
arg1 = "collision_model_full:=true"
arg2 = "collision_model_simplified:=false"
arg3 = "collision_model_enlargement:=0.0"
arg4 = "collision_model_no_hands:=false"
subprocess.call(["rosrun", "xacro", "xacro", "-o", urdf_uri, xacro_uri, arg1, arg2, arg3, arg4])
robot_name = self.urdf_module.SendCommand('load ' + urdf_uri + ' ' + srdf_uri )
print "robot name: " + robot_name
self.openrave_robot = self.env.GetRobot(robot_name)
self.env.Remove(self.openrave_robot)
for mimic in mimic_joints:
mj = self.openrave_robot.GetJoint(mimic[0])
mj.SetMimicEquations(0, mimic[1], mimic[2], mimic[3])
self.openrave_robot.GetActiveManipulator().SetChuckingDirection([0,0,0,0])#directions)
self.env.Add(self.openrave_robot)
self.openrave_robot = self.env.GetRobot(robot_name)
# print "robots: "
# print self.env.GetRobots()
joint_names = []
print "active joints:"
for j in self.openrave_robot.GetJoints():
joint_names.append(j.GetName())
print j
print "passive joints:"
for j in self.openrave_robot.GetPassiveJoints():
joint_names.append(j.GetName())
print j
# ODE does not support distance measure
self.env.GetCollisionChecker().SetCollisionOptions(CollisionOptions.Contacts)
self.env.Add(self.openrave_robot)
vertices, faces = surfaceutils.readStl(rospack.get_path('velma_scripts') + "/data/meshes/klucz_gerda_ascii.stl", scale=1.0)
# vertices, faces = surfaceutils.readStl("klucz_gerda_ascii.stl", scale=1.0)
self.addTrimesh("object", vertices, faces)
# self.addBox("object", 0.2,0.06,0.06)
# self.addSphere("object", 0.15)
# vertices, faces = self.getMesh("object")
#
# definition of the expected external wrenches
#
ext_wrenches = []
# origin of the external wrench (the end point of the key)
wr_orig = PyKDL.Vector(0.039, 0.0, 0.0)
for i in range(8):
# expected force at the end point
force = PyKDL.Frame(PyKDL.Rotation.RotX(float(i)/8.0 * 2.0 * math.pi)) * PyKDL.Vector(0,1,0)
# expected torque at the com
torque = wr_orig * force
ext_wrenches.append(PyKDL.Wrench(force, torque))
# expected force at the end point
force = PyKDL.Frame(PyKDL.Rotation.RotX(float(i)/8.0 * 2.0 * math.pi)) * PyKDL.Vector(-1,1,0)
# expected torque at the com
torque = wr_orig * force
ext_wrenches.append(PyKDL.Wrench(force, torque))
#
# definition of the grasps
#
grasp_id = 0
if grasp_id == 0:
grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2
grasp_initial_configuration = [60.0/180.0*math.pi, None, None, None]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.122103662206, -0.124395758838, -0.702726011729, 0.689777190329), PyKDL.Vector(-0.00115787237883, -0.0194999426603, 0.458197712898))
# self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.174202588426, -0.177472708083, -0.691231954612, 0.678495061771), PyKDL.Vector(0.0, -0.0213436260819, 0.459123969078))
elif grasp_id == 1:
grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, None, None, None]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0187387771868, -0.708157209758, -0.0317875569224, 0.705090018033), PyKDL.Vector(4.65661287308e-10, 0.00145332887769, 0.472836345434))
elif grasp_id == 2:
grasp_direction = [0, 1, 0, 0] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, None, 90.0/180.0*math.pi, 0]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0187387763947, -0.708157179826, -0.0317875555789, 0.705089928626), PyKDL.Vector(0.0143095180392, 0.00145332887769, 0.483659058809))
elif grasp_id == 3:
grasp_direction = [0, 1, 1, 1] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, None, None, None]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00518634245761, -0.706548316769, -0.0182458505507, 0.707410947861), PyKDL.Vector(0.000126354629174, -0.00217361748219, 0.47637796402))
elif grasp_id == 4:
grasp_direction = [0, 0, 1, 0] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, 100.0/180.0*math.pi, None, 100.0/180.0*math.pi]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.153445252933, -0.161230275653, 0.681741576082, 0.696913201022), PyKDL.Vector(0.000126355327666, 0.00152841210365, 0.466048002243))
elif grasp_id == 5:
grasp_direction = [0, 0, 1, 0] # spread, f1, f3, f2
grasp_initial_configuration = [100.0/180.0*math.pi, 101.5/180.0*math.pi, None, 101.5/180.0*math.pi]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.155488650062, -0.159260521271, 0.690572597636, 0.688163302213), PyKDL.Vector(-0.000278688268736, 0.00575117766857, 0.461560428143))
elif grasp_id == 6:
grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, None, 0, 0]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.512641041738, -0.485843507183, -0.514213889193, 0.48655882699), PyKDL.Vector(-0.000278423947748, -0.00292747467756, 0.445628076792))
else:
print "ERROR: unknown grasp_id: %s"%(grasp_id)
exit(0)
self.updatePose("object", self.T_W_O)
with open('barret_hand_openrave2ros_joint_map2.txt', 'r') as f:
lines = f.readlines()
joint_map = {}
for line in lines:
line_s = line.split()
if len(line_s) == 2:
joint_map[line_s[0]] = line_s[1]
elif len(line_s) != 1:
print "error in joint map file"
exit(0)
print joint_map
self.pub_js = rospy.Publisher("/joint_states", JointState)
if True:
def getDirectionIndex(n):
min_angle = -45.0/180.0*math.pi
angle_range = 90.0/180.0*math.pi
angles_count = 5
angles_count2 = angles_count * angles_count
max_indices = angles_count2 * 6
if abs(n.x()) > abs(n.y()) and abs(n.x()) > abs(n.z()):
if n.x() > 0:
sec = 0
a1 = math.atan2(n.y(), n.x())
a2 = math.atan2(n.z(), n.x())
else:
sec = 1
a1 = math.atan2(n.y(), -n.x())
a2 = math.atan2(n.z(), -n.x())
elif abs(n.y()) > abs(n.x()) and abs(n.y()) > abs(n.z()):
if n.y() > 0:
sec = 2
a1 = math.atan2(n.x(), n.y())
a2 = math.atan2(n.z(), n.y())
else:
sec = 3
a1 = math.atan2(n.x(), -n.y())
a2 = math.atan2(n.z(), -n.y())
else:
if n.z() > 0:
sec = 4
a1 = math.atan2(n.x(), n.z())
a2 = math.atan2(n.y(), n.z())
else:
sec = 5
a1 = math.atan2(n.x(), -n.z())
a2 = math.atan2(n.y(), -n.z())
a1i = int(angles_count*(a1-min_angle)/angle_range)
a2i = int(angles_count*(a2-min_angle)/angle_range)
if a1i < 0:
print sec, a1i, a2i
a1i = 0
if a1i >= angles_count:
# print sec, a1i, a2i
a1i = angles_count-1
if a2i < 0:
print sec, a1i, a2i
a2i = 0
if a2i >= angles_count:
print sec, a1i, a2i
a2i = angles_count-1
return sec * angles_count2 + a1i * angles_count + a2i
# generate a dictionary of indexed directions
normals_sphere_indexed_dir = velmautils.generateNormalsSphere(3.0/180.0*math.pi)
print len(normals_sphere_indexed_dir)
indices_directions = {}
for n in normals_sphere_indexed_dir:
index = getDirectionIndex(n)
if not index in indices_directions:
indices_directions[index] = [n]
else:
indices_directions[index].append(n)
for index in indices_directions:
n_mean = PyKDL.Vector()
for n in indices_directions[index]:
n_mean += n
n_mean.Normalize()
indices_directions[index] = n_mean
# test direction indexing
if False:
normals_sphere = velmautils.generateNormalsSphere(3.0/180.0*math.pi)
print len(normals_sphere)
m_id = 0
for current_index in range(5*5*6):
count = 0
r = random.random()
g = random.random()
b = random.random()
for n in normals_sphere:
sec = -1
a1 = None
a2 = None
index = getDirectionIndex(n)
if index == current_index:
m_id = self.pub_marker.publishSinglePointMarker(n*0.1, m_id, r=r, g=g, b=b, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.007, 0.007, 0.007), T=None)
count += 1
print current_index, count
for index in indices_directions:
m_id = self.pub_marker.publishSinglePointMarker(indices_directions[index]*0.12, m_id, r=1, g=1, b=1, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.012, 0.012, 0.012), T=None)
exit(0)
#
# PyODE test
#
if True:
fixed_joints_names_for_fixed_DOF = [
["right_HandFingerOneKnuckleOneJoint", "right_HandFingerTwoKnuckleOneJoint"], # spread
["right_HandFingerOneKnuckleTwoJoint", "right_HandFingerOneKnuckleThreeJoint"], # f1
["right_HandFingerThreeKnuckleTwoJoint", "right_HandFingerThreeKnuckleThreeJoint"], # f3
["right_HandFingerTwoKnuckleTwoJoint", "right_HandFingerTwoKnuckleThreeJoint"], # f2
]
coupled_joint_names_for_fingers = ["right_HandFingerOneKnuckleThreeJoint", "right_HandFingerTwoKnuckleThreeJoint", "right_HandFingerThreeKnuckleThreeJoint"]
actuated_joints_for_DOF = [
["right_HandFingerOneKnuckleOneJoint", "right_HandFingerTwoKnuckleOneJoint"], # spread
["right_HandFingerOneKnuckleTwoJoint"], # f1
["right_HandFingerThreeKnuckleTwoJoint"], # f3
["right_HandFingerTwoKnuckleTwoJoint"]] # f2
ignored_links = ["world", "world_link"]
self.run_ode_simulation = False
self.insert6DofGlobalMarker(self.T_W_O)
print "grasp_direction = [%s, %s, %s, %s] # spread, f1, f3, f2"%(grasp_direction[0], grasp_direction[1], grasp_direction[2], grasp_direction[3])
print "grasp_initial_configuration = [%s, %s, %s, %s]"%(grasp_initial_configuration[0], grasp_initial_configuration[1], grasp_initial_configuration[2], grasp_initial_configuration[3])
print "grasp_final_configuration = %s"%(self.openrave_robot.GetDOFValues())
rot_q = self.T_W_O.M.GetQuaternion()
print "self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s, %s, %s, %s), PyKDL.Vector(%s, %s, %s))"%(rot_q[0], rot_q[1], rot_q[2], rot_q[3], self.T_W_O.p.x(), self.T_W_O.p.y(), self.T_W_O.p.z())
self.erase6DofMarker()
raw_input("Press ENTER to continue...")
# reset the gripper in Openrave
self.openrave_robot.SetDOFValues([0,0,0,0])
# read the position of all links
grasp_links_poses = {}
for link in self.openrave_robot.GetLinks():
grasp_links_poses[link.GetName()] = self.OpenraveToKDL(link.GetTransform())
# print "obtained the gripper configuration for the grasp:"
# print finalconfig[0]
#
# simulation in ODE
#
# Create a world object
world = ode.World()
# world.setGravity( (0,0,-3.81) )
world.setGravity( (0,0,0) )
CFM = world.getCFM()
ERP = world.getERP()
print "CFM: %s ERP: %s"%(CFM, ERP)
# world.setCFM(0.001)
# print "CFM: %s ERP: %s"%(CFM, ERP)
self.space = ode.Space()
# Create a body inside the world
body = ode.Body(world)
M = ode.Mass()
M.setCylinderTotal(0.02, 1, 0.005, 0.09)
body.setMass(M)
ode_mesh = ode.TriMeshData()
ode_mesh.build(vertices, faces)
geom = ode.GeomTriMesh(ode_mesh, self.space)
geom.name = "object"
geom.setBody(body)
self.setOdeBodyPose(geom, self.T_W_O)
ode_gripper_geoms = {}
for link in self.openrave_robot.GetLinks():
link_name = link.GetName()
if link_name in ignored_links:
print "ignoring: %s"%(link_name)
continue
print "adding: %s"%(link_name)
ode_mesh_link = None
body_link = None
T_W_L = self.OpenraveToKDL(link.GetTransform())
col = link.GetCollisionData()
vertices = col.vertices
faces = col.indices
ode_mesh_link = ode.TriMeshData()
ode_mesh_link.build(vertices, faces)
ode_gripper_geoms[link_name] = ode.GeomTriMesh(ode_mesh_link, self.space)
if True:
body_link = ode.Body(world)
M_link = ode.Mass()
M_link.setCylinderTotal(0.05, 1, 0.01, 0.09)
body_link.setMass(M_link)
ode_gripper_geoms[link_name].setBody(body_link)
ode_gripper_geoms[link_name].name = link.GetName()
self.setOdeBodyPose(body_link, T_W_L)
actuated_joint_names = []
for dof in range(4):
for joint_name in actuated_joints_for_DOF[dof]:
actuated_joint_names.append(joint_name)
ode_gripper_joints = {}
for joint_name in joint_names:
joint = self.openrave_robot.GetJoint(joint_name)
link_parent = joint.GetHierarchyParentLink().GetName()
link_child = joint.GetHierarchyChildLink().GetName()
if link_parent in ode_gripper_geoms and link_child in ode_gripper_geoms:
axis = joint.GetAxis()
anchor = joint.GetAnchor()
if joint_name in actuated_joint_names:
ode_gripper_joints[joint_name] = ode.AMotor(world)
ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody())
ode_gripper_joints[joint_name].setMode(ode.AMotorUser)#Euler)
# ode_gripper_joints[joint_name].setNumAxes(3)
ode_gripper_joints[joint_name].setAxis(0,1,axis)
# ode_gripper_joints[joint_name].setAxis(1,1,axis)
# ode_gripper_joints[joint_name].setAxis(2,1,axis)
ode_gripper_joints[joint_name].setParam(ode.ParamFMax, 10.0)
# ode_gripper_joints[joint_name].setParam(ode.ParamFMax2, 10.0)
# ode_gripper_joints[joint_name].setParam(ode.ParamFMax3, 10.0)
else:
ode_gripper_joints[joint_name] = ode.HingeJoint(world)
ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody())
ode_gripper_joints[joint_name].setAxis(-axis)
ode_gripper_joints[joint_name].setAnchor(anchor)
# ode_gripper_joints[joint_name] = ode.HingeJoint(world)
# ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody())
# ode_gripper_joints[joint_name].setAxis(-axis)
# ode_gripper_joints[joint_name].setAnchor(anchor)
# if joint_name in actuated_joint_names:
# ode_gripper_joints[joint_name].setParam(ode.ParamFMax, 10.0)
limits = joint.GetLimits()
value = joint.GetValue(0)
# print joint_name
# print "limits: %s %s"%(limits[0], limits[1])
# print "axis: %s"%(axis)
# print "anchor: %s"%(anchor)
# print "value: %s"%(value)
lim = [limits[0], limits[1]]
if limits[0] <= -math.pi:
# print "lower joint limit %s <= -PI, setting to -PI+0.01"%(limits[0])
lim[0] = -math.pi + 0.01
if limits[1] >= math.pi:
# print "upper joint limit %s >= PI, setting to PI-0.01"%(limits[1])
lim[1] = math.pi - 0.01
ode_gripper_joints[joint_name].setParam(ode.ParamLoStop, lim[0])
ode_gripper_joints[joint_name].setParam(ode.ParamHiStop, lim[1])
# ode_gripper_joints[joint_name].setParam(ode.ParamFudgeFactor, 0.5)
ode_fixed_joint = ode.FixedJoint(world)
ode_fixed_joint.attach(None, ode_gripper_geoms["right_HandPalmLink"].getBody())
ode_fixed_joint.setFixed()
#
# set the poses of all links as for the grasp
#
for link_name in grasp_links_poses:
if link_name in ignored_links:
continue
ode_body = ode_gripper_geoms[link_name].getBody()
T_W_L = grasp_links_poses[link_name]
self.setOdeBodyPose(ode_body, T_W_L)
fixed_joint_names = []
fixed_joint_names += coupled_joint_names_for_fingers
for dof in range(4):
if grasp_direction[dof] == 0.0:
for joint_name in fixed_joints_names_for_fixed_DOF[dof]:
if not joint_name in fixed_joint_names:
fixed_joint_names.append(joint_name)
#
# change all coupled joints to fixed joints
#
fixed_joints = {}
for joint_name in fixed_joint_names:
# save the bodies attached
body0 = ode_gripper_joints[joint_name].getBody(0)
body1 = ode_gripper_joints[joint_name].getBody(1)
# save the joint angle
if joint_name in actuated_joint_names:
angle = ode_gripper_joints[joint_name].getAngle(0)
else:
angle = ode_gripper_joints[joint_name].getAngle()
# detach the joint
ode_gripper_joints[joint_name].attach(None, None)
del ode_gripper_joints[joint_name]
fixed_joints[joint_name] = [ode.FixedJoint(world), angle]
fixed_joints[joint_name][0].attach(body0, body1)
fixed_joints[joint_name][0].setFixed()
# change all actuated joints to motor joints
# actuated_joint_names = []
# for dof in range(4):
# for joint_name in actuated_joints_for_DOF[dof]:
# actuated_joint_names.append(joint_name)
# for joint_name in actuated_joint_names:
# A joint group for the contact joints that are generated whenever
# two bodies collide
contactgroup = ode.JointGroup()
print "ode_gripper_geoms:"
print ode_gripper_geoms
initial_T_W_O = self.T_W_O
# Do the simulation...
dt = 0.001
total_time = 0.0
failure = False
#
# PID
#
Kc = 1.0
# Ti = 1000000000000.0
KcTi = 0.0
Td = 0.0
Ts = 0.001
pos_d = 1.0
e0 = 0.0
e1 = 0.0
e2 = 0.0
u = 0.0
u_max = 1.0
while total_time < 5.0 and not rospy.is_shutdown():
#
# ODE simulation
#
joint = ode_gripper_joints["right_HandFingerOneKnuckleTwoJoint"]
e2 = e1
e1 = e0
e0 = pos_d - joint.getAngle(0)
u = u + Kc * (e0 - e1) + KcTi * Ts * e0 + Kc * Td / Ts * (e0 - 2.0 * e1 + e2)
if u > u_max:
u = u_max
if u < -u_max:
u = -u_max
joint.setParam(ode.ParamFMax, 10000.0)
joint.setParam(ode.ParamVel, 1.1)
# joint.setParam(ode.ParamVel2, 1.1)
# joint.setParam(ode.ParamVel3, 1.1)
print joint.getAngle(0), " ", u #, " ", joint.getAngleRate(0)
# print u
# joint.addTorque(u)
# for dof in range(4):
# for joint_name in actuated_joints_for_DOF[dof]:
# if joint_name in ode_gripper_joints:
# ode_gripper_joints[joint_name].addTorque(1*grasp_direction[dof])
self.grasp_contacts = []
self.space.collide((world,contactgroup), self.near_callback)
world.step(dt)
total_time += dt
contactgroup.empty()
#
# ROS interface
#
old_m_id = m_id
m_id = 0
# publish frames from ODE
if False:
for link_name in ode_gripper_geoms:
link_body = ode_gripper_geoms[link_name].getBody()
if link_body == None:
link_body = ode_gripper_geoms[link_name]
T_W_Lsim = self.getOdeBodyPose(link_body)
m_id = self.pub_marker.publishFrameMarker(T_W_Lsim, m_id, scale=0.05, frame='world', namespace='default')
# publish the mesh of the object
T_W_Osim = self.getOdeBodyPose(body)
m_id = self.pub_marker.publishConstantMeshMarker("package://velma_scripts/data/meshes/klucz_gerda_binary.stl", m_id, r=1, g=0, b=0, scale=1.0, frame_id='world', namespace='default', T=T_W_Osim)
# update the gripper visualization in ros
js = JointState()
js.header.stamp = rospy.Time.now()
for jn in joint_map:
ros_joint_name = joint_map[jn]
js.name.append(ros_joint_name)
if jn in ode_gripper_joints:
if jn in actuated_joint_names:
js.position.append( ode_gripper_joints[jn].getAngle(0) )
else:
js.position.append( ode_gripper_joints[jn].getAngle() )
elif jn in fixed_joints:
js.position.append(fixed_joints[jn][1])
else:
js.position.append(0)
self.pub_js.publish(js)
# draw contacts
for c in self.grasp_contacts:
cc = PyKDL.Vector(c[0], c[1], c[2])
cn = PyKDL.Vector(c[3], c[4], c[5])
m_id = self.pub_marker.publishVectorMarker(cc, cc+cn*0.04, m_id, 1, 0, 0, frame='world', namespace='default', scale=0.003)
if m_id < old_m_id:
self.pub_marker.eraseMarkers(m_id,old_m_id+1, frame_id='world')
diff_T_W_O = PyKDL.diff(initial_T_W_O, T_W_Osim)
# if diff_T_W_O.vel.Norm() > 0.02 or diff_T_W_O.rot.Norm() > 30.0/180.0*math.pi:
# print "the object moved"
# print diff_T_W_O
# failure = True
# break
rospy.sleep(0.01)
if not failure:
T_O_Wsim = T_W_Osim.Inverse()
TR_O_Wsim = PyKDL.Frame(T_O_Wsim.M)
contacts = []
for c in self.grasp_contacts:
cc_W = PyKDL.Vector(c[0], c[1], c[2])
cn_W = PyKDL.Vector(c[3], c[4], c[5])
cc_O = T_O_Wsim * cc_W
cn_O = TR_O_Wsim * cn_W
contacts.append([cc_O[0], cc_O[1], cc_O[2], cn_O[0], cn_O[1], cn_O[2]])
gws, contact_planes = self.generateGWS(contacts, 1.0)
grasp_quality = None
for wr in ext_wrenches:
wr_qual = self.getQualityMeasure2(gws, wr)
if grasp_quality == None or wr_qual < grasp_quality:
grasp_quality = wr_qual
grasp_quality_classic = self.getQualityMeasure(gws)
print "grasp_quality_classic: %s grasp_quality: %s"%(grasp_quality_classic, grasp_quality)
exit(0)
def spin(self):
rospack = rospkg.RosPack()
urdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator.urdf'
srdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator.srdf'
dyn_model = planar5.DynModelPlanar5()
col = collision_model.CollisionModel()
col.readUrdfSrdf(urdf_path, srdf_path)
self.joint_names = ["0_joint", "1_joint", "2_joint", "3_joint", "4_joint"]
effector_name = 'effector'
ndof = len(self.joint_names)
# robot state
self.q = np.zeros( ndof )
self.dq = np.zeros( ndof )
self.q[0] = 0.2
self.q[1] = -0.1
self.q[2] = -0.1
self.q[3] = -0.1
self.q[4] = -0.1
solver = fk_ik.FkIkSolver(self.joint_names, [], None)
self.publishJointState()
# obstacles
obst = []
obj = collision_model.CollisionModel.Collision()
obj.type = "capsule"
obj.T_L_O = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi), PyKDL.Vector(1.5, 0.7, 0))
obj.radius = 0.1
obj.length = 0.4
obst.append( obj )
obj = collision_model.CollisionModel.Collision()
obj.type = "capsule"
obj.T_L_O = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi), PyKDL.Vector(1.2, -0.9, 0))
obj.radius = 0.1
obj.length = 0.4
obst.append( obj )
obj = collision_model.CollisionModel.Collision()
obj.type = "sphere"
obj.T_L_O = PyKDL.Frame(PyKDL.Vector(1, -0.2, 0))
obj.radius = 0.05
obst.append( obj )
last_time = rospy.Time.now()
limit_range = np.zeros( ndof )
max_trq = np.zeros( ndof )
for q_idx in range( ndof ):
limit_range[q_idx] = 15.0/180.0*math.pi
max_trq[q_idx] = 10.0
dyn_model.computeM(self.q)
obst_offset = 0.0
counter = 10000
while not rospy.is_shutdown():
obst_offset += 0.0001
obst[-1].T_L_O = PyKDL.Frame(PyKDL.Vector(1, -0.2+math.sin(obst_offset), 0))
if counter > 800:
r_HAND_target = PyKDL.Frame(PyKDL.Rotation.RotZ(random.uniform(-math.pi, math.pi)), PyKDL.Vector(random.uniform(0,2), random.uniform(-1,1), 0))
qt = r_HAND_target.M.GetQuaternion()
pt = r_HAND_target.p
print "**** STATE ****"
print "r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s,%s,%s,%s), PyKDL.Vector(%s,%s,%s))"%(qt[0],qt[1],qt[2],qt[3],pt[0],pt[1],pt[2])
print "self.q = np.array(", self.q, ")"
print "self.dq = np.array(", self.dq, ")"
counter = 0
counter += 1
time_elapsed = rospy.Time.now() - last_time
#
# mass matrix
#
dyn_model.computeM(self.q)
M = dyn_model.M
Minv = dyn_model.Minv
#
# JLC
#
torque_JLC = np.zeros( ndof )
K = np.zeros( (ndof, ndof) )
for q_idx in range( ndof ):
torque_JLC[q_idx] = self.jointLimitTrq(solver.lim_upper[q_idx], solver.lim_lower[q_idx], limit_range[q_idx], max_trq[q_idx], self.q[q_idx])
if abs(torque_JLC[q_idx]) > 0.001:
K[q_idx,q_idx] = max_trq[q_idx]/limit_range[q_idx]
else:
K[q_idx,q_idx] = 0.001
w, v = scipy.linalg.eigh(a=K, b=M)
# q_ = dyn_model.gaussjordan(np.matrix(v))
q_ = np.linalg.inv( np.matrix(v) )
k0_ = w
k0_sqrt = np.zeros( k0_.shape )
for q_idx in range( ndof ):
k0_sqrt[q_idx] = math.sqrt(k0_[q_idx])
tmpNN_ = np.diag(k0_sqrt)
D = 2.0 * q_.H * 0.7 * tmpNN_ * q_
torque_JLC_mx = -D * np.matrix(self.dq).transpose()
for q_idx in range( ndof ):
torque_JLC[q_idx] += torque_JLC_mx[q_idx, 0]
# calculate jacobian (the activation function)
J_JLC = np.matrix(numpy.zeros( (ndof, ndof) ))
for q_idx in range( ndof ):
if self.q[q_idx] < solver.lim_lower_soft[q_idx]:
J_JLC[q_idx,q_idx] = min(1.0, 10*abs(self.q[q_idx] - solver.lim_lower_soft[q_idx]) / abs(solver.lim_lower[q_idx] - solver.lim_lower_soft[q_idx]))
elif self.q[q_idx] > solver.lim_upper_soft[q_idx]:
J_JLC[q_idx,q_idx] = min(1.0, 10*abs(self.q[q_idx] - solver.lim_upper_soft[q_idx]) / abs(solver.lim_upper[q_idx] - solver.lim_upper_soft[q_idx]))
else:
J_JLC[q_idx,q_idx] = 0.0
N_JLC = np.matrix(np.identity( ndof )) - (J_JLC.transpose() * J_JLC)
links_fk = {}
for link in col.links:
links_fk[link.name] = solver.calculateFk('base', link.name, self.q)
#
# collision constraints
#
link_collision_map = {}
if True:
activation_dist = 0.05
# self collision
total_contacts = 0
for link1_name, link2_name in col.collision_pairs:
link1 = col.link_map[link1_name]
T_B_L1 = links_fk[link1_name]
link2 = col.link_map[link2_name]
T_B_L2 = links_fk[link2_name]
for col1 in link1.col:
for col2 in link2.col:
T_B_C1 = T_B_L1 * col1.T_L_O
T_B_C2 = T_B_L2 * col2.T_L_O
c_dist = (T_B_C1 * PyKDL.Vector() - T_B_C2 * PyKDL.Vector()).Norm()
if col1.type == "capsule":
c_dist -= col1.radius + col1.length/2
elif col1.type == "sphere":
c_dist -= col1.radius
if col2.type == "capsule":
c_dist -= col2.radius + col2.length/2
elif col2.type == "sphere":
c_dist -= col2.radius
if c_dist > activation_dist:
continue
dist = None
if col1.type == "capsule" and col2.type == "capsule":
line1 = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0))
line2 = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0))
dist, p1_B, p2_B = self.distanceLines(line1, line2)
elif col1.type == "capsule" and col2.type == "sphere":
line = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0))
pt = T_B_C2 * PyKDL.Vector()
dist, p1_B, p2_B = self.distanceLinePoint(line, pt)
elif col1.type == "sphere" and col2.type == "capsule":
pt = T_B_C1 * PyKDL.Vector()
line = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0))
dist, p1_B, p2_B = self.distancePointLine(pt, line)
elif col1.type == "sphere" and col2.type == "sphere":
dist, p1_B, p2_B = self.distancePoints(T_B_C1 * PyKDL.Vector(), T_B_C2 * PyKDL.Vector())
else:
print "ERROR: unknown collision type:", col1.type, col2.type
exit(0)
if dist != None:
dist -= col1.radius + col2.radius
v = p2_B - p1_B
v.Normalize()
n1_B = v
n2_B = -v
p1_B += n1_B * col1.radius
p2_B += n2_B * col2.radius
if dist < activation_dist:
if not (link1_name, link2_name) in link_collision_map:
link_collision_map[(link1_name, link2_name)] = []
link_collision_map[(link1_name, link2_name)].append( (p1_B, p2_B, dist, n1_B, n2_B) )
# environment collisions
for link in col.links:
if link.col == None:
continue
link1_name = link.name
T_B_L1 = links_fk[link1_name]
T_B_L2 = links_fk["base"]
for col1 in link.col:
for col2 in obst:
T_B_C1 = T_B_L1 * col1.T_L_O
T_B_C2 = T_B_L2 * col2.T_L_O
dist = None
if col1.type == "capsule" and col2.type == "capsule":
line1 = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0))
line2 = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0))
dist, p1_B, p2_B = self.distanceLines(line1, line2)
elif col1.type == "capsule" and col2.type == "sphere":
line = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0))
pt = T_B_C2 * PyKDL.Vector()
dist, p1_B, p2_B = self.distanceLinePoint(line, pt)
elif col1.type == "sphere" and col2.type == "capsule":
pt = T_B_C1 * PyKDL.Vector()
line = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0))
dist, p1_B, p2_B = self.distancePointLine(pt, line)
elif col1.type == "sphere" and col2.type == "sphere":
dist, p1_B, p2_B = self.distancePoints(T_B_C1 * PyKDL.Vector(), T_B_C2 * PyKDL.Vector())
else:
print "ERROR: unknown collision type:", col1.type, col2.type
exit(0)
if dist != None:
dist -= col1.radius + col2.radius
v = p2_B - p1_B
v.Normalize()
n1_B = v
n2_B = -v
p1_B += n1_B * col1.radius
p2_B += n2_B * col2.radius
if dist < activation_dist:
if not (link1_name, "base") in link_collision_map:
link_collision_map[(link1_name, "base")] = []
link_collision_map[(link1_name, "base")].append( (p1_B, p2_B, dist, n1_B, n2_B) )
torque_col = np.matrix(np.zeros( (ndof,1) ))
Ncol = np.matrix(np.identity( ndof ))
m_id = 0
for link1_name, link2_name in link_collision_map:
for (p1_B, p2_B, dist, n1_B, n2_B) in link_collision_map[ (link1_name, link2_name) ]:
if dist > 0.0:
m_id = self.pub_marker.publishVectorMarker(p1_B, p2_B, m_id, 1, 1, 1, frame='base', namespace='default', scale=0.02)
else:
m_id = self.pub_marker.publishVectorMarker(p1_B, p2_B, m_id, 1, 0, 0, frame='base', namespace='default', scale=0.02)
T_B_L1 = links_fk[link1_name]
T_L1_B = T_B_L1.Inverse()
T_B_L2 = links_fk[link2_name]
T_L2_B = T_B_L2.Inverse()
p1_L1 = T_L1_B * p1_B
p2_L2 = T_L2_B * p2_B
n1_L1 = PyKDL.Frame(T_L1_B.M) * n1_B
n2_L2 = PyKDL.Frame(T_L2_B.M) * n2_B
# print p1_L1, p1_L1+n1_L1*0.2
# print p2_L2, p2_L2+n2_L2*0.2
# m_id = self.pub_marker.publishVectorMarker(p1_L1, p1_L1+n1_L1*0.2, m_id, 0, 1, 0, frame=link1_name, namespace='default', scale=0.02)
# m_id = self.pub_marker.publishVectorMarker(T_B_L2*p2_L2, T_B_L2*(p2_L2+n2_L2*0.2), m_id, 0, 0, 1, frame="base", namespace='default', scale=0.02)
jac1 = PyKDL.Jacobian( ndof )
jac2 = PyKDL.Jacobian( ndof )
common_link_name = solver.getJacobiansForPairX(jac1, jac2, link1_name, p1_L1, link2_name, p2_L2, self.q, None)
# print link1_name, link2_name
depth = (activation_dist - dist)
# repulsive force
Fmax = 20.0
if dist <= activation_dist:
f = (dist - activation_dist) / activation_dist
else:
f = 0.0
Frep = Fmax * f * f
K = 2.0 * Fmax / (activation_dist * activation_dist)
# the mapping between motions along contact normal and the Cartesian coordinates
e1 = n1_L1
e2 = n2_L2
Jd1 = np.matrix([e1[0], e1[1], e1[2]])
Jd2 = np.matrix([e2[0], e2[1], e2[2]])
# rewrite the linear part of the jacobian
jac1_mx = np.matrix(np.zeros( (3, ndof) ))
jac2_mx = np.matrix(np.zeros( (3, ndof) ))
for q_idx in range(ndof):
col1 = jac1.getColumn(q_idx)
col2 = jac2.getColumn(q_idx)
for row_idx in range(3):
jac1_mx[row_idx, q_idx] = col1[row_idx]
jac2_mx[row_idx, q_idx] = col2[row_idx]
Jcol1 = Jd1 * jac1_mx
Jcol2 = Jd2 * jac2_mx
Jcol = np.matrix(np.zeros( (1, ndof) ))
for q_idx in range(ndof):
Jcol[0, q_idx] = Jcol1[0, q_idx] + Jcol2[0, q_idx]
# calculate relative velocity between points
ddij = Jcol * np.matrix(self.dq).transpose()
activation = min(1.0, 5.0*depth/activation_dist)
activation = max(0.0, activation)
if ddij <= 0.0:
activation = 0.0
a_des = activation
# print "activation", activation
# raw_input(".")
if rospy.is_shutdown():
exit(0)
Ncol12 = np.matrix(np.identity(ndof)) - (Jcol.transpose() * a_des * Jcol)
Ncol = Ncol * Ncol12
# calculate collision mass
Mdij = Jcol * Minv * Jcol.transpose()
D = 2.0 * 0.7 * math.sqrt(Mdij * K)
d_torque = Jcol.transpose() * (-Frep - D * ddij)
torque_col += d_torque
self.pub_marker.eraseMarkers(m_id, 10, frame_id='base', namespace='default')
#
# end-effector task
#
T_B_E = links_fk[effector_name]
r_HAND_current = T_B_E
diff = PyKDL.diff(r_HAND_current, r_HAND_target)
r_HAND_diff = np.array( [diff[0], diff[1], diff[5]] )
Kc = np.array( [10, 10, 1] )
Dxi = np.array( [0.7, 0.7, 0.7] )
wrench = np.zeros( 3 )
for dim in range(3):
wrench[dim] = Kc[dim] * r_HAND_diff[dim]
J_r_HAND_6 = solver.getJacobian('base', effector_name, self.q)
J_r_HAND = np.matrix( np.zeros( (3,5) ) )
for q_idx in range( ndof ):
J_r_HAND[0, q_idx] = J_r_HAND_6[0, q_idx]
J_r_HAND[1, q_idx] = J_r_HAND_6[1, q_idx]
J_r_HAND[2, q_idx] = J_r_HAND_6[5, q_idx]
torque_HAND = J_r_HAND.transpose() * np.matrix(wrench).transpose()
A = J_r_HAND * Minv * J_r_HAND.transpose()
# A = dyn_model.gaussjordan(A)
A = np.linalg.inv(A)
tmpKK_ = np.matrix(np.diag(Kc))
w, v = scipy.linalg.eigh(a=tmpKK_, b=A)
# Q = dyn_model.gaussjordan(np.matrix(v))
Q = np.linalg.inv( np.matrix(v) )
K0 = w
Dc = Q.transpose() * np.matrix( np.diag(Dxi) )
K0_sqrt = np.zeros( K0.shape )
for dim_idx in range( 3 ):
K0_sqrt[dim_idx] = math.sqrt(K0[dim_idx])
Dc = 2.0 * Dc * np.matrix( np.diag(K0_sqrt) ) * Q
F = Dc * J_r_HAND * np.matrix(self.dq).transpose()
torque_HAND_mx = -J_r_HAND.transpose() * F
for q_idx in range( ndof ):
torque_HAND[q_idx] += torque_HAND_mx[q_idx, 0]
# null-space
# tmpNK_.noalias() = J * Mi;
# tmpKK_.noalias() = tmpNK_ * JT;
# luKK_.compute(tmpKK_);
# tmpKK_ = luKK_.inverse();
# tmpKN_.noalias() = Mi * JT;
# Ji.noalias() = tmpKN_ * tmpKK_;
#
# P.noalias() = Eigen::MatrixXd::Identity(P.rows(), P.cols());
# P.noalias() -= J.transpose() * A * J * Mi;
torque = np.matrix(torque_JLC).transpose() + N_JLC.transpose() * (torque_col + (Ncol.transpose() * torque_HAND))
# torque = torque_HAND
time_d = 0.01
# simulate one step
ddq = dyn_model.accel(self.q, self.dq, torque)
for q_idx in range(ndof):
self.dq[q_idx] += ddq[q_idx,0] * time_d
self.q[q_idx] += self.dq[q_idx] * time_d
if time_elapsed.to_sec() > 0.05:
m_id = 0
m_id = self.publishRobotModelVis(m_id, col, links_fk)
m_id = self.publishObstaclesVis(m_id, obst)
last_time = rospy.Time.now()
self.publishJointState()
self.publishTransform(r_HAND_target, "hand_dest")
T_C_M = []
T_T2_7 = []
T_C_M_stable = PyKDL.Frame()
T_T2_7_stable = PyKDL.Frame()
stable_t = 0
while True:
rospy.sleep(0.1)
try:
pose = tf_listener.lookupTransform('camera', 'ar_marker_'+str(right_makrer_id), rospy.Time(0))
T_C_M_current = pm.fromTf(pose)
pose = tf_listener.lookupTransform('head_tilt_link', 'right_arm_7_link', rospy.Time(0))
T_T2_7_current = pm.fromTf(pose)
except:
continue
d1 = PyKDL.diff(T_C_M_stable, T_C_M_current)
d2 = PyKDL.diff(T_T2_7_stable, T_T2_7_current)
score = d2.vel.Norm() + d2.rot.Norm()
if score > 0.002:
stable_t = 0
T_C_M_stable = copy.deepcopy(T_C_M_current)
T_T2_7_stable = copy.deepcopy(T_T2_7_current)
else:
stable_t += 1
add = True
if stable_t > 10:
for t in T_T2_7:
d = PyKDL.diff(T_T2_7_current, t)
if d.rot.Norm() < 0.1:
add = False
break
def poseUpdaterThread(self, args, *args2):
index = 0
z_limit = 0.3
while not rospy.is_shutdown():
rospy.sleep(0.1)
if self.allow_update_objects_pose == None or not self.allow_update_objects_pose:
continue
for obj_name in self.dyn_obj_markers:
obj = self.dyn_obj_markers[obj_name]
visible_markers_Br_Co = []
visible_markers_weights_ori = []
visible_markers_weights_pos = []
visible_markers_idx = []
for marker in obj:#.markers:
T_Br_M = self.getMarkerPose(marker[0], wait = False, timeBack = 0.3)
if T_Br_M != None and self.velma != None:
T_B_C = self.getCameraPose()
T_C_M = T_B_C.Inverse() * T_Br_M
v = T_C_M * PyKDL.Vector(0,0,1) - T_C_M * PyKDL.Vector()
if v.z() > -z_limit:
continue
# v.z() is in range (-1.0, -0.3)
weight = ((-v.z()) - z_limit)/(1.0-z_limit)
if weight > 1.0 or weight < 0.0:
print "error: weight==%s"%(weight)
T_Co_M = marker[1]
T_Br_Co = T_Br_M * T_Co_M.Inverse()
visible_markers_Br_Co.append(T_Br_Co)
visible_markers_weights_ori.append(1.0-weight)
visible_markers_weights_pos.append(weight)
visible_markers_idx.append(marker[0])
if len(visible_markers_Br_Co) > 0:
# if obj.name == "object":
# print "vis: %s"%(visible_markers_idx)
# print "w_o: %s"%(visible_markers_weights_ori)
# print "w_p: %s"%(visible_markers_weights_pos)
# first calculate mean pose without weights
R_B_Co = velmautils.meanOrientation(visible_markers_Br_Co)[1]
p_B_Co = velmautils.meanPosition(visible_markers_Br_Co)
T_B_Co = PyKDL.Frame(copy.deepcopy(R_B_Co.M), copy.deepcopy(p_B_Co))
distances = []
for m_idx in range(0, len(visible_markers_Br_Co)):
diff = PyKDL.diff(T_B_Co, visible_markers_Br_Co[m_idx])
distances.append( [diff, m_idx] )
Br_Co_list = []
weights_ori = []
weights_pos = []
for d in distances:
if d[0].vel.Norm() > 0.04 or d[0].rot.Norm() > 15.0/180.0*math.pi:
continue
Br_Co_list.append( visible_markers_Br_Co[d[1]] )
weights_ori.append( visible_markers_weights_ori[d[1]] )
weights_pos.append( visible_markers_weights_pos[d[1]] )
if len(Br_Co_list) > 0:
R_B_Co = velmautils.meanOrientation(Br_Co_list, weights=weights_ori)[1]
p_B_Co = velmautils.meanPosition(Br_Co_list, weights=weights_pos)
# obj.updatePose( PyKDL.Frame(copy.deepcopy(R_B_Co.M), copy.deepcopy(p_B_Co)) )
self.openrave.updatePose(obj_name, T_Br_Co)
index += 1
if index >= 100:
index = 0
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_diff(frame1, frame2, dt):
return (
kdltwist_to_narray(
PyKDL.diff(
narray_to_kdlframe(frame1), narray_to_kdlframe(frame2), dt)))
def spin(self):
simulation_only = False
key_endpoint_T = PyKDL.Vector(0,0,0.2)
T_B_P = PyKDL.Frame(PyKDL.Rotation(), PyKDL.Vector(1,0,1))
m_id = 0
self.pub_marker.eraseMarkers(0,3000, frame_id='world')
print "creating interface for Velma..."
# create the interface for Velma robot
self.velma = Velma()
print "done."
m_id = 0
self.pub_marker.eraseMarkers(0,3000, frame_id='world')
# key and grasp parameters
self.T_E_O = PyKDL.Frame()
self.T_E_Orot = PyKDL.Frame(PyKDL.Vector(0, 0, 0.07))
self.key_axis_O = PyKDL.Vector(0,0,1)
self.key_up_O = PyKDL.Vector(1,0,0)
self.key_side_O = self.key_axis_O * self.key_up_O
# change the tool - the safe way
print "switching to joint impedance..."
if not self.velma.switchToJoint():
print "ERROR: switchToJoint"
exit(0)
rospy.sleep(0.5)
print "updating tool..."
self.velma.updateTransformations()
self.velma.updateAndMoveToolOnly(PyKDL.Frame(self.velma.T_W_E.p+PyKDL.Vector(0.1,0,0)), 0.1)
rospy.sleep(0.5)
print "done."
print "switching to cartesian impedance..."
if not self.velma.switchToCart():
print "ERROR: switchToCart"
exit(0)
rospy.sleep(0.5)
raw_input("Press ENTER to continue...")
q_grasp = [1.4141768883517725, 1.4179811607057289, 1.4377081536379384, 0]
q_pre = 10.0/180.0*math.pi
hv = [1.2, 1.2, 1.2, 1.2]
ht = [3000, 3000, 3000, 3000]
# set gripper configuration
if True:
self.velma.moveHand([0.0, 0.0, 0.0, 0.0/180.0*math.pi], hv, ht, 5000, True)
rospy.sleep(3)
self.velma.moveHand([q_grasp[0]-q_pre, q_grasp[1]-q_pre, q_grasp[2]-q_pre, q_grasp[3]], hv, ht, 5000, True)
rospy.sleep(5)
self.velma.moveHand([q_grasp[0]+q_pre, q_grasp[1]+q_pre, q_grasp[2]+q_pre, q_grasp[3]], hv, ht, 5000, True)
rospy.sleep(3)
if False:
# start with very low stiffness
print "setting stiffness to very low value"
k_low = Wrench(Vector3(1.0, 1.0, 1.0), Vector3(0.5, 0.5, 0.5))
self.velma.moveImpedance(k_low, 0.5)
if self.velma.checkStopCondition(0.5):
exit(0)
print "done."
print "identifying the parameters of tool in..."
wait_time = 20
for i in range(wait_time):
print "%s s"%(wait_time-i)
rospy.sleep(1.0)
print "collecting points..."
self.collect_poses = True
T_B_T_list = []
thread.start_new_thread(self.posesCollectorThread, (T_B_T_list,))
collect_time = 10
for i in range(collect_time):
print "%s s"%(collect_time-i)
rospy.sleep(1.0)
self.collect_poses = False
rospy.sleep(0.5)
key_endpoint_T, error = self.estCommonPoint(T_B_T_list)
print key_endpoint_T, error
self.key_endpoint_O = self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() * self.velma.T_W_T * key_endpoint_T
print "self.key_endpoint_O = PyKDL.Vector(%s, %s, %s)"%(self.key_endpoint_O[0], self.key_endpoint_O[1], self.key_endpoint_O[2])
print "done."
else:
# self.key_endpoint_O = PyKDL.Vector(-0.00248664992634, -6.7348683886e-05, 0.232163117525)
self.key_endpoint_O = PyKDL.Vector(0.000256401261281, -0.000625166847342, 0.232297442735)
k_high = Wrench(Vector3(1000.0, 1000.0, 1000.0), Vector3(150, 150, 150))
max_force = 12.0
max_torque = 12.0
path_tol = (PyKDL.Vector(max_force/k_high.force.x, max_force/k_high.force.y, max_force/k_high.force.z), PyKDL.Vector(max_torque/k_high.torque.x, max_torque/k_high.torque.y, max_torque/k_high.torque.z))
max_force2 = 20.0
max_torque2 = 20.0
path_tol2 = (PyKDL.Vector(max_force2/k_high.force.x, max_force2/k_high.force.y, max_force2/k_high.force.z), PyKDL.Vector(max_torque2/k_high.torque.x, max_torque2/k_high.torque.y, max_torque2/k_high.torque.z))
path_tol3 = (PyKDL.Vector(max_force/k_high.force.x, max_force/k_high.force.y, max_force2/k_high.force.z), PyKDL.Vector(max_torque/k_high.torque.x, max_torque/k_high.torque.y, max_torque/k_high.torque.z))
print "path tolerance:", path_tol
# k_hole_in = Wrench(Vector3(1000.0, 500.0, 500.0), Vector3(200, 200, 200))
k_hole_in = Wrench(Vector3(100.0, 1000.0, 1000.0), Vector3(50, 5, 5))
path_tol_in = (PyKDL.Vector(max_force2/k_hole_in.force.x, max_force/k_hole_in.force.y, max_force/k_hole_in.force.z), PyKDL.Vector(max_torque2/k_hole_in.torque.x, max_torque2/k_hole_in.torque.y, max_torque2/k_hole_in.torque.z))
path_tol_in2 = (PyKDL.Vector(max_force2/k_hole_in.force.x, max_force2/k_hole_in.force.y, max_force2/k_hole_in.force.z), PyKDL.Vector(max_torque2/k_hole_in.torque.x, max_torque2/k_hole_in.torque.y, max_torque2/k_hole_in.torque.z))
if False:
k_increase = [
Wrench(Vector3(10.0, 10.0, 10.0), Vector3(5, 5, 5)),
Wrench(Vector3(50.0, 50.0, 50.0), Vector3(25, 25, 25)),
Wrench(Vector3(250.0, 250.0, 250.0), Vector3(100, 100, 100))
]
for k in k_increase:
raw_input("Press Enter to set bigger stiffness...")
self.velma.updateTransformations()
T_B_Wd = self.velma.T_B_W
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
print "moving the wrist to the current pose in " + str(time) + " s..."
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)))
rospy.sleep(time)
print "done."
print "setting stiffness to bigger value"
self.velma.moveImpedance(k, 1.0)
if self.velma.checkStopCondition(1.1):
exit(0)
print "done."
print "move the grasped key near the key hole..."
self.follow_wrist_pose = True
thread.start_new_thread(self.wristFollowerThread, (None,))
raw_input("Press ENTER to save the pose...")
self.follow_wrist_pose = False
rospy.sleep(0.5)
self.velma.updateTransformations()
self.T_B_O_nearHole = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O
p = self.T_B_O_nearHole.p
q = self.T_B_O_nearHole.M.GetQuaternion()
print "self.T_B_O_nearHole = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s, %s, %s, %s), PyKDL.Vector(%s, %s, %s))"%(q[0], q[1], q[2], q[3], p[0], p[1], p[2])
T_B_Wd = self.velma.T_B_W
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
print "moving the wrist to the current pose in " + str(time) + " s..."
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)))
status, feedback = self.velma.waitForCartEnd()
print "setting stiffness to", k_high
self.velma.moveImpedance(k_high, 1.0)
if self.velma.checkStopCondition(1.0):
exit(0)
print "done."
else:
# self.T_B_O_nearHole = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.697525674378, -0.00510212356955, 0.715527762697, 0.0381041038336), PyKDL.Vector(0.528142123375, 0.0071159410235, 1.31300679435))
# self.T_B_O_nearHole = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.699716675653, -0.0454110538496, 0.709529999372, 0.0700113561626), PyKDL.Vector(0.546491646893, -0.101297899801, 1.30959887442))
self.T_B_O_nearHole = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.71891504857, -0.0529880479354, 0.691118088949, 0.0520500417212), PyKDL.Vector(0.883081316461, -0.100813768303, 0.95381559114))
k_increase = [
Wrench(Vector3(10.0, 10.0, 10.0), Vector3(5, 5, 5)),
Wrench(Vector3(50.0, 50.0, 50.0), Vector3(25, 25, 25)),
Wrench(Vector3(250.0, 250.0, 250.0), Vector3(100, 100, 100)),
k_high
]
for k in k_increase:
raw_input("Press Enter to set bigger stiffness...")
self.velma.updateTransformations()
T_B_Wd = self.velma.T_B_W
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
print "moving the wrist to the current pose in " + str(time) + " s..."
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)))
rospy.sleep(time)
print "done."
print "setting stiffness to bigger value"
self.velma.moveImpedance(k, 1.0)
if self.velma.checkStopCondition(1.1):
exit(0)
print "done."
if True:
print "probing the door..."
search_radius = 0.02
min_dist = 0.005
probed_points = []
contact_points_B = []
# move to the center point and push the lock
x = 0
y = 0
T_O_Od = PyKDL.Frame(self.key_up_O*x + self.key_side_O*y)
T_O_Od2 = PyKDL.Frame(self.key_up_O*x + self.key_side_O*y + self.key_axis_O*0.1)
self.velma.updateTransformations()
T_B_Wd = self.T_B_O_nearHole * T_O_Od * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2)
status, feedback = self.velma.waitForCartEnd()
print "status", status
if status.error_code != 0:
print "unexpected contact", feedback
exit(0)
self.velma.updateTransformations()
T_B_Wd = self.T_B_O_nearHole * T_O_Od2 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol)
status, feedback = self.velma.waitForCartEnd()
print "status", status
if status.error_code != 0:
self.velma.updateTransformations()
contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O
contact_points_B.append(contact_B)
m_id = self.pub_marker.publishSinglePointMarker(contact_B, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
print feedback
else:
print "no contact"
exit(0)
T_O_Od3 = PyKDL.Frame(self.key_axis_O * 5.0/k_high.force.z)
self.velma.updateTransformations()
T_B_Wref = self.velma.T_B_W
T_B_Wd = T_B_Wref * self.velma.T_W_E * self.T_E_O * T_O_Od3 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2)
status, feedback = self.velma.waitForCartEnd()
print "status", status
if status.error_code != 0:
print "unexpected high contact force", feedback
exit(0)
desired_push_dist = 5.0/k_high.force.z
self.velma.updateTransformations()
contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O
contact_points_B.append(contact_B)
# move along the spiral
hole_found = False
spiral = self.generateSpiral(search_radius, min_dist)
for pt in spiral:
pt_desired_O_in_ref = self.key_endpoint_O
pt_contact_O_in_ref = self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() * T_B_Wref.Inverse() * contact_B
key_end_depth = PyKDL.dot(pt_contact_O_in_ref-pt_desired_O_in_ref, self.key_axis_O)
# print "key_end_depth", key_end_depth
T_O_Od4 = PyKDL.Frame(self.key_up_O*pt[0] + self.key_side_O*pt[1] + self.key_axis_O * (5.0/k_high.force.z + key_end_depth))
pt_desired_B = T_B_Wref * self.velma.T_W_E * self.T_E_O * T_O_Od4 * self.key_endpoint_O
m_id = self.pub_marker.publishSinglePointMarker(pt_desired_B, m_id, r=0, g=1, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
T_B_Wd = T_B_Wref * self.velma.T_W_E * self.T_E_O * T_O_Od4 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol)
status, feedback = self.velma.waitForCartEnd()
print "status", status
self.velma.updateTransformations()
contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O
contact_points_B.append(contact_B)
m_id = self.pub_marker.publishSinglePointMarker(contact_B, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
# check if the key is in the hole
if status.error_code != 0:
self.velma.updateTransformations()
T_O_Od5 = PyKDL.Frame(self.key_axis_O * 5.0/k_high.force.z)
T_B_Wd = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * T_O_Od5 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2)
status, feedback = self.velma.waitForCartEnd()
if status.error_code != 0:
print "unexpected high contact force", feedback
exit(0)
self.velma.updateTransformations()
T_O_Od6 = PyKDL.Frame(self.key_up_O*0.02 + self.key_axis_O * 5.0/k_high.force.z)
T_B_Wd = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * T_O_Od6 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol)
status, feedback = self.velma.waitForCartEnd()
if status.error_code != 0:
print "key is in a hole"
hole_found = True
break
print "hole_found", hole_found
T_O_Od3 = PyKDL.Frame(self.key_axis_O * 5.0/k_high.force.z)
self.velma.updateTransformations()
T_B_Wref = self.velma.T_B_W
print "moving the wrist to the current pose to reduce tension..."
T_B_Wd = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * T_O_Od3 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2)
status, feedback = self.velma.waitForCartEnd()
print "status", status
if status.error_code != 0:
print "unexpected high contact force", feedback
exit(0)
print "setting stiffness to bigger value"
self.velma.moveImpedance(k_hole_in, 1.0)
if self.velma.checkStopCondition(1.1):
exit(0)
print "done."
self.velma.updateTransformations()
# calculate the fixed lock frame T_B_L
# T_B_L the same orientation as the gripper (z axis pointing towards the door)
# and the origin at the key endpoint at the initial penetration
T_B_L = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * PyKDL.Frame(self.key_endpoint_O)
penetration_axis_L = PyKDL.Vector(0, 0, 1)
# get current contact point
contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O
contact_L = T_B_L.Inverse() * contact_B
contact_max_depth_L = PyKDL.dot(contact_L, penetration_axis_L)
contact_depth_L = contact_max_depth_L
prev_contact_depth_L = contact_depth_L
deepest_contact_L = copy.deepcopy(contact_L)
m_id = self.pub_marker.publishSinglePointMarker(contact_B, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
central_point_L = PyKDL.Vector()
force_key_axis = 17.0
force_key_up = 10.0
force_key_side = 10.0
xi = 7
yi = 7
explore_mode = "get_new"
while True:
# desired position of the key end
self.velma.updateTransformations()
T_B_Ocurrent = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * PyKDL.Frame(self.key_axis_O * (force_key_axis/k_hole_in.force.x))
diff_B = PyKDL.diff(T_B_Ocurrent, self.T_B_O_nearHole)
rot_diff_Ocurrent = PyKDL.Frame(T_B_Ocurrent.Inverse().M) * diff_B.rot
spiral_hole = self.generateSpiral(0.04, 0.005)
T_B_W_shake = []
for pt in spiral_hole:
T_B_Od_shake = T_B_Ocurrent * PyKDL.Frame(PyKDL.Rotation.RotZ(rot_diff_Ocurrent.z()-6.0/180.0*math.pi), self.key_up_O * pt[0] + self.key_side_O * pt[1])
T_B_W_shake.append(T_B_Od_shake * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse())
T_B_Od_shake = T_B_Ocurrent * PyKDL.Frame(PyKDL.Rotation.RotZ(rot_diff_Ocurrent.z()+6.0/180.0*math.pi), self.key_up_O * pt[0] + self.key_side_O * pt[1])
T_B_W_shake.append(T_B_Od_shake * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse())
print "shaking..."
counter = 0
for T_B_W in T_B_W_shake:
counter += 1
time = self.velma.getMovementTime(T_B_W, max_v_l = 0.4, max_v_r = 0.4)
# self.velma.moveWrist2(T_B_W * self.velma.T_W_T)
# raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_W, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol_in)
status1, feedback = self.velma.waitForCartEnd()
print "status", status1
self.velma.updateTransformations()
contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O
m_id = self.pub_marker.publishSinglePointMarker(contact_B, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
pt_desired_B = T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O
m_id = self.pub_marker.publishSinglePointMarker(pt_desired_B, m_id, r=0, g=1, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
contact_L = T_B_L.Inverse() * contact_B
contact_depth_L = PyKDL.dot(contact_L, penetration_axis_L)
if contact_depth_L > contact_max_depth_L+0.0001:
deepest_contact_L = copy.deepcopy(contact_L)
contact_max_depth_L = contact_depth_L
print "contact_depth_L: %s"%(contact_depth_L)
explore_mode = "push"
break
if status1.error_code != 0:
break
print "done."
score = contact_depth_L - prev_contact_depth_L
prev_contact_depth_L = contact_depth_L
if status1.error_code != 0 or counter == len(T_B_W_shake):
break
print "moving the key to the current pose..."
self.velma.updateTransformations()
T_B_Wd = self.velma.T_B_W
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.1, max_v_r = 0.1)
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol_in2)
status1, feedback = self.velma.waitForCartEnd()
print "status", status1
if status1.error_code != 0:
exit(0)
print "moving the key to the current pose..."
self.velma.updateTransformations()
T_B_Ocurrent = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O
T_B_Od = T_B_Ocurrent * PyKDL.Frame(self.key_axis_O * (-2.0/k_hole_in.force.x))
T_B_Wd = T_B_Od * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.1, max_v_r = 0.1)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol_in)
status1, feedback = self.velma.waitForCartEnd()
print "status", status1
if status1.error_code != 0:
exit(0)
self.velma.moveHand([q_grasp[0]-q_pre, q_grasp[1]-q_pre, q_grasp[2]-q_pre, q_grasp[3]], hv, ht, 5000, True)
rospy.sleep(2)
print "setting stiffness to bigger value"
self.velma.moveImpedance(k_high, 1.0)
if self.velma.checkStopCondition(1.1):
exit(0)
print "done."
T_B_Wd = T_B_Ocurrent * self.T_E_Orot.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.02)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2)
status1, feedback = self.velma.waitForCartEnd()
print "status", status1
if status1.error_code != 0:
exit(0)
self.velma.moveHand([q_grasp[0]+q_pre, q_grasp[1]+q_pre, q_grasp[2]+q_pre, q_grasp[3]], hv, ht, 5000, True)
rospy.sleep(5)
# m_id = self.pub_marker.publishSinglePointMarker(T_B_L * PyKDL.Vector(0.01*xi+ori_map_vis_offset[0], 0.01*yi+ori_map_vis_offset[1], score), m_id, r=0, g=0, b=1, a=0.5, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
# m_id = self.pub_marker.publishSinglePointMarker(T_B_L * PyKDL.Vector(0.01*xi+ori_map_vis_offset[0], 0.01*yi+ori_map_vis_offset[1], ori_map[xi][yi][1]), m_id, r=0, g=1, b=0, a=0.5, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
exit(0)
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
