def spin(self):
sys.setrecursionlimit(200)
# generate the set of grasps for a jar
grasps_T_J_E = []
for angle_jar_axis in np.arange(0.0, math.pi*2.0, 10.0/180.0*math.pi):
for translation_jar_axis in np.linspace(-0.03, 0.03, 7):
T_J_E = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi)) * PyKDL.Frame(PyKDL.Rotation.RotX(angle_jar_axis)) * PyKDL.Frame(PyKDL.Vector(translation_jar_axis, 0, -0.17))
grasps_T_J_E.append( (T_J_E, T_J_E.Inverse()) )
T_J_E = PyKDL.Frame(PyKDL.Rotation.RotZ(-90.0/180.0*math.pi)) * PyKDL.Frame(PyKDL.Rotation.RotX(angle_jar_axis)) * PyKDL.Frame(PyKDL.Vector(translation_jar_axis, 0, -0.17))
grasps_T_J_E.append( (T_J_E, T_J_E.Inverse()) )
# TEST: OpenJarTaskRRT
if False:
task = tasks.OpenJarTaskRRT(None, ("right", grasps_T_J_E, ))
m_id = 0
for coord in task.valid:
xi,yi,zi = coord
x = plutl.x_set[xi]
y = plutl.y_set[yi]
z = plutl.z_set[zi]
rot_set = task.valid[coord]
size = float(len(rot_set))/40.0
m_id = self.pub_marker.publishSinglePointMarker(PyKDL.Vector(x,y,z), m_id, r=1.0, g=1*size, b=1*size, a=1, namespace='default', frame_id="torso_link2", m_type=Marker.SPHERE, scale=Vector3(0.05*size, 0.05*size, 0.05*size))
rospy.sleep(0.01)
exit(0)
rospack = rospkg.RosPack()
env_file=rospack.get_path('velma_scripts') + '/data/common/velma_room.env.xml'
# env_file=rospack.get_path('velma_scripts') + '/data/common/velma_room_obstacles.env.xml'
srdf_path=rospack.get_path('velma_description') + '/robots/'
obj_filenames = [
rospack.get_path('velma_scripts') + '/data/jar/jar.kinbody.xml'
]
rrt = rrt_star_connect_planner.PlannerRRT(3, env_file, obj_filenames, srdf_path)
rospy.wait_for_service('/change_state')
changeState = rospy.ServiceProxy('/change_state', state_server_msgs.srv.ChangeState)
self.listener = tf.TransformListener()
print "creating interface for Velma..."
# create the interface for Velma robot
velma = Velma()
self.pub_head_look_at = rospy.Publisher("/head_lookat_pose", geometry_msgs.msg.Pose)
print "done."
rospy.sleep(0.5)
velma.updateTransformations()
# switch to cartesian impedance mode...
# velma.switchToCart()
# exit(0)
velma.switchToJoint()
# TEST: moving in joint impedance mode
if False:
raw_input("Press ENTER to move the robot...")
velma.switchToJoint()
joint_names = [ "right_arm_4_joint" ]
q_dest = [ velma.js_pos["right_arm_4_joint"] - 10.0/180.0*math.pi ]
velma.moveJoint(q_dest, joint_names, 10.0, start_time=0.5)
traj = [[q_dest], None, None, [10.0]]
velma.moveJointTraj(traj, joint_names, start_time=0.5)
result = velma.waitForJoint()
print "moveJointTraj result", result.error_code
exit(0)
hv = [1.2, 1.2, 1.2, 1.2]
ht = [3000, 3000, 3000, 3000]
target_gripper = "right"
if target_gripper == "left":
velma.moveHandLeft([40.0/180.0*math.pi, 40.0/180.0*math.pi, 40.0/180.0*math.pi, 0], hv, ht, 5000, True)
velma.moveHandRight([120.0/180.0*math.pi, 120.0/180.0*math.pi, 120.0/180.0*math.pi, 0], hv, ht, 5000, True)
else:
velma.moveHandLeft([120.0/180.0*math.pi, 120.0/180.0*math.pi, 120.0/180.0*math.pi, 0], hv, ht, 5000, True)
velma.moveHandRight([40.0/180.0*math.pi, 40.0/180.0*math.pi, 40.0/180.0*math.pi, 0], hv, ht, 5000, True)
result = velma.waitForHandLeft()
print "waitForHandLeft result", result.error_code
result = velma.waitForHandRight()
print "waitForHandRight result", result.error_code
#
# Initialise Openrave
#
openrave = openraveinstance.OpenraveInstance()
openrave.startOpenrave(collision='fcl')
openrave.loadEnv(env_file)
openrave.runOctomapClient(ros=True)
openrave.readRobot(srdf_path=srdf_path)
for filename in obj_filenames:
body = openrave.env.ReadKinBodyXMLFile(filename)
body.Enable(False)
body.SetVisible(False)
openrave.env.Add(body)
mo_state = objectstate.MovableObjectsState(openrave.env, obj_filenames)
openrave.setCamera(PyKDL.Vector(2.0, 0.0, 2.0), PyKDL.Vector(0.60, 0.0, 1.10))
openrave.updateRobotConfigurationRos(velma.js_pos)
while True:
if self.listener.canTransform('torso_base', 'jar', rospy.Time(0)):
pose = self.listener.lookupTransform('torso_base', 'jar', rospy.Time(0))
T_B_J = pm.fromTf(pose)
break
print "waiting for planner..."
rrt.waitForInit()
print "planner initialized"
T_B_E_list = []
for T_J_E, T_E_J in grasps_T_J_E:
T_B_Ed = T_B_J * T_J_E
T_B_E_list.append(T_B_Ed)
print "grasps for jar:", len(T_B_E_list)
if True:
# look around
self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,0,1.2))))
rospy.sleep(3)
# self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,-1,1.2))))
# rospy.sleep(2)
# self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,1,1.2))))
# rospy.sleep(2)
# self.pub_head_look_at.publish(pm.toMsg(PyKDL.Frame(PyKDL.Vector(1,0,1.2))))
# rospy.sleep(2)
raw_input("Press ENTER to start planning...")
openrave.updateOctomap()
tree_serialized = openrave.or_octomap_client.SendCommand("GetOcTree")
for i in range(10):
time_tf = rospy.Time.now()-rospy.Duration(0.5)
mo_state.update(self.listener, time_tf)
mo_state.updateOpenrave(openrave.env)
rospy.sleep(0.1)
print "octomap updated"
print "Planning trajectory to grasp the jar..."
env_state = (openrave.robot_rave.GetDOFValues(), mo_state.obj_map, tree_serialized, [])
path, dof_names = rrt.RRTstar(env_state, tasks.GraspTaskRRT, (target_gripper, T_B_E_list), 240.0)
traj = []
for i in range(len(path)-1):
q1 = path[i]
q2 = path[i+1]
for f in np.linspace(0.0, 1.0, 40):
traj.append( q1 * (1.0 - f) + q2 * f )
while True:
if raw_input("Type e to execute") == 'e':
break
openrave.showTrajectory(dof_names, 10.0, traj)
traj = velma.prepareTrajectory(path, velma.getJointStatesByNames(dof_names), dof_names, speed_mult=1.0)
velma.switchToJoint()
velma.moveJointTraj(traj, dof_names, start_time=0.5)
result = velma.waitForJoint()
print "moveJointTraj result", result.error_code
if result.error_code != 0:
exit(0)
raw_input("Press ENTER to continue...")
openrave.updateRobotConfigurationRos(velma.js_pos)
print "before grasp"
report = CollisionReport()
if openrave.env.CheckCollision(openrave.robot_rave, report):
print "collision"
else:
print "no collision"
report = CollisionReport()
if openrave.robot_rave.CheckSelfCollision(report):
print "self-collision"
else:
print "no self-collision"
raw_input("Press ENTER to close the gripper...")
if target_gripper == "left":
velma.moveHandLeft([90.0/180.0*math.pi, 90.0/180.0*math.pi, 90.0/180.0*math.pi, 0], hv, ht, 5000, True)
result = velma.waitForHandLeft()
print "moveHandLeft result", result.error_code
else:
velma.moveHandRight([90.0/180.0*math.pi, 90.0/180.0*math.pi, 90.0/180.0*math.pi, 0], hv, ht, 5000, True)
result = velma.waitForHandRight()
print "waitForHandRight result", result.error_code
openrave.updateRobotConfigurationRos(velma.js_pos)
print "after grasp"
report = CollisionReport()
if openrave.env.CheckCollision(openrave.robot_rave, report):
print "collision"
else:
print "no collision"
report = CollisionReport()
if openrave.robot_rave.CheckSelfCollision(report):
print "self-collision"
else:
print "no self-collision"
openrave.robot_rave.Grab(openrave.env.GetKinBody("jar"), openrave.robot_rave.GetLink(target_gripper+"_HandPalmLink"))
changeState("grasp "+target_gripper+"_HandPalmLink "+ "jar")
print "after grab"
report = CollisionReport()
if openrave.env.CheckCollision(openrave.robot_rave, report):
print "collision"
else:
print "no collision"
report = CollisionReport()
if openrave.robot_rave.CheckSelfCollision(report):
print "self-collision"
else:
print "no self-collision"
print "Planning trajectory to pull the jar from the cabinet..."
for i in range(10):
time_tf = rospy.Time.now()-rospy.Duration(0.5)
mo_state.update(self.listener, time_tf)
mo_state.updateOpenrave(openrave.env)
rospy.sleep(0.1)
env_state = (openrave.robot_rave.GetDOFValues(), mo_state.obj_map, tree_serialized, [["jar", target_gripper+"_HandPalmLink"]])
# path, dof_names = rrt.RRTstar(env_state, tasks.GraspTaskRRT, (target_gripper, T_B_E_list), 60.0)
path, dof_names = rrt.RRTstar(env_state, tasks.MoveArmsCloseTaskRRT, (None,), 120.0)
# path.reverse()
traj = []
for i in range(len(path)-1):
q1 = path[i]
q2 = path[i+1]
for f in np.linspace(0.0, 1.0, 40):
traj.append( q1 * (1.0 - f) + q2 * f )
while True:
if raw_input("Type e to execute") == 'e':
break
openrave.showTrajectory(dof_names, 10.0, traj)
traj = velma.prepareTrajectory(path, velma.getJointStatesByNames(dof_names), dof_names, speed_mult=1.0)
result = velma.moveJointTraj(traj, dof_names, start_time=0.5)
result = velma.waitForJoint()
print "moveJointTraj result", result.error_code
if result.error_code != 0:
exit(0)
raw_input("Press ENTER to continue...")
openrave.updateRobotConfigurationRos(velma.js_pos)
changeState("drop "+target_gripper+"_HandPalmLink")
raw_input("Press ENTER to exit...")
rrt.cleanup()
def spin(self):
if False:
while not rospy.is_shutdown():
pt = PyKDL.Vector(random.uniform(-1, 1), random.uniform(-1, 1), 0)
# line = (PyKDL.Vector(0,0,0), PyKDL.Vector(1,0,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.distanceLinePoint(line, pt)
dist, p1, p2 = self.distanceLines(line, line2)
m_id = 0
m_id = self.pub_marker.publishVectorMarker(line[0], line[1], m_id, 1, 0, 0, frame='world', namespace='default', scale=0.02)
m_id = self.pub_marker.publishVectorMarker(line2[0], line2[1], m_id, 0, 1, 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, pt, dist
print line, line2, dist
raw_input(".")
exit(0)
simulation = True
rospack = rospkg.RosPack()
env_file=rospack.get_path('velma_scripts') + '/data/jar/cabinet_test.env.xml'
srdf_path=rospack.get_path('velma_description') + '/robots/'
print "creating interface for Velma..."
# create the interface for Velma robot
velma = Velma()
print "done."
##
velma.waitForInit()
velma.fk_ik_solver.createSegmentToJointMap(velma.getJointStatesVectorNames(), velma.getInactiveJointStatesVectorNames())
q = velma.getJointStatesVector()
iq = velma.getInactiveJointStatesVector()
jac1 = PyKDL.Jacobian(len(q))
jac2 = PyKDL.Jacobian(len(q))
velma.fk_ik_solver.getJacobiansForPairX(jac1, jac2, "left_HandPalmLink", PyKDL.Vector(), "torso_link1", PyKDL.Vector(0.1,0,0), q, iq)
print "jac1"
print jac1
print "jac2"
print jac2
exit(0)
##
#
# Initialise Openrave
#
openrave = openraveinstance.OpenraveInstance()
openrave.startOpenraveURDF(env_file=env_file, viewer=True)
openrave.readRobot(srdf_path=srdf_path)
openrave.setCamera(PyKDL.Vector(2.0, 0.0, 2.0), PyKDL.Vector(0.60, 0.0, 1.10))
velma.waitForInit()
openrave.updateRobotConfigurationRos(velma.js_pos)
non_adj_links_ids = openrave.robot_rave.GetNonAdjacentLinks()
velma.switchToJoint()
lim_bo_soft, lim_up_soft = velma.getJointSoftLimitsVectors()
lim_bo, lim_up = velma.getJointLimitsVectors()
velma.fk_ik_solver.createJacobianFkSolvers('torso_base', 'right_HandPalmLink', velma.getJointStatesVectorNames())
velma.fk_ik_solver.createJacobianFkSolvers('torso_base', 'left_HandPalmLink', velma.getJointStatesVectorNames())
velma.fk_ik_solver.createSegmentToJointMap(velma.getJointStatesVectorNames(), velma.getInactiveJointStatesVectorNames())
print velma.getJointStatesVectorNames()
r_HAND_targets = [
# PyKDL.Frame(PyKDL.Vector(0.5,0,1.8)),
# PyKDL.Frame(PyKDL.Rotation.RotY(170.0/180.0*math.pi), PyKDL.Vector(0.5,0,1.6)),
PyKDL.Frame(PyKDL.Rotation.RotY(90.0/180.0*math.pi), PyKDL.Vector(0.2,0.0,1.0)),
PyKDL.Frame(PyKDL.Rotation.RotY(90.0/180.0*math.pi), PyKDL.Vector(0.2,-0.5,1.0)),
]
l_HAND_targets = [
# PyKDL.Frame(PyKDL.Vector(0.5,0,1.8)),
# PyKDL.Frame(PyKDL.Rotation.RotY(170.0/180.0*math.pi), PyKDL.Vector(0.5,0,1.6)),
PyKDL.Frame(PyKDL.Rotation.RotY(90.0/180.0*math.pi) * PyKDL.Rotation.RotZ(180.0/180.0*math.pi), PyKDL.Vector(0.2,0.0,1.0)),
PyKDL.Frame(PyKDL.Rotation.RotY(90.0/180.0*math.pi) * PyKDL.Rotation.RotZ(180.0/180.0*math.pi), PyKDL.Vector(0.2,0.5,1.0)),
]
target_idx = 0
r_HAND_target = r_HAND_targets[target_idx]
l_HAND_target = l_HAND_targets[target_idx]
target_idx += 1
last_time = rospy.Time.now()
q = velma.getJointStatesVector()
q_names = velma.getJointStatesVectorNames()
iq = velma.getInactiveJointStatesVector()
counter = 0
while not rospy.is_shutdown():
if counter > 300:
r_HAND_target = r_HAND_targets[target_idx]
l_HAND_target = l_HAND_targets[target_idx]
target_idx = (target_idx + 1)%len(r_HAND_targets)
counter = 0
counter += 1
time_elapsed = rospy.Time.now() - last_time
J_JLC = np.matrix(numpy.zeros( (len(q), len(q)) ))
delta_V_JLC = np.empty(len(q))
for q_idx in range(len(q)):
if q[q_idx] < lim_bo_soft[q_idx]:
delta_V_JLC[q_idx] = q[q_idx] - lim_bo_soft[q_idx]
J_JLC[q_idx,q_idx] = min(1.0, 10*abs(q[q_idx] - lim_bo_soft[q_idx]) / abs(lim_bo[q_idx] - lim_bo_soft[q_idx]))
elif q[q_idx] > lim_up_soft[q_idx]:
delta_V_JLC[q_idx] = q[q_idx] - lim_up_soft[q_idx]
J_JLC[q_idx,q_idx] = min(1.0, 10*abs(q[q_idx] - lim_up_soft[q_idx]) / abs(lim_up[q_idx] - lim_up_soft[q_idx]))
else:
delta_V_JLC[q_idx] = 0.0
J_JLC[q_idx,q_idx] = 0.0
J_JLC_inv = np.linalg.pinv(J_JLC)
N_JLC = identityMatrix(len(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 = 1.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
# right hand
J_r_HAND = velma.fk_ik_solver.getJacobian('torso_base', 'right_HandPalmLink', q)
J_r_HAND_inv = np.linalg.pinv(J_r_HAND)
N_r_HAND = identityMatrix(len(q)) - (J_r_HAND_inv * J_r_HAND)
T_B_E = velma.fk_ik_solver.calculateFk2('torso_base', 'right_HandPalmLink', q)
r_HAND_current = T_B_E
r_HAND_diff = PyKDL.diff(r_HAND_target, r_HAND_current)
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 = 2.0
kp_HAND = 2.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
# left hand
J_l_HAND = velma.fk_ik_solver.getJacobian('torso_base', 'left_HandPalmLink', q)
J_l_HAND_inv = np.linalg.pinv(J_l_HAND)
N_l_HAND = identityMatrix(len(q)) - (J_l_HAND_inv * J_l_HAND)
T_B_E = velma.fk_ik_solver.calculateFk2('torso_base', 'left_HandPalmLink', q)
l_HAND_current = T_B_E
l_HAND_diff = PyKDL.diff(l_HAND_target, l_HAND_current)
delta_V_HAND = np.empty(6)
delta_V_HAND[0] = l_HAND_diff.vel[0]
delta_V_HAND[1] = l_HAND_diff.vel[1]
delta_V_HAND[2] = l_HAND_diff.vel[2]
delta_V_HAND[3] = l_HAND_diff.rot[0]
delta_V_HAND[4] = l_HAND_diff.rot[1]
delta_V_HAND[5] = l_HAND_diff.rot[2]
v_max_HAND = 2.0
kp_HAND = 2.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_l_HAND_ref = - vv_HAND * dx_HAND_des
link_collision_map = {}
openrave.updateRobotConfiguration(q, velma.getJointStatesVectorNames())
if True:
openrave.switchCollisionModel("velmasimplified1")
col_chk = openrave.env.GetCollisionChecker()
col_opt = col_chk.GetCollisionOptions()
col_chk.SetCollisionOptions(0x04) # CO_Contacts(0x04), CO_AllLinkCollisions(0x20)
total_contacts = 0
for link1_idx, link2_idx in non_adj_links_ids:
link1 = openrave.robot_rave.GetLinks()[link1_idx]
link2 = openrave.robot_rave.GetLinks()[link2_idx]
report = CollisionReport()
if col_chk.CheckCollision(link1=link1, link2=link2, report=report):
T_L1_W = conv.OpenraveToKDL(link1.GetTransform()).Inverse()
TR_L1_W = PyKDL.Frame(T_L1_W.M)
T_L2_W = conv.OpenraveToKDL(link2.GetTransform()).Inverse()
TR_L2_W = PyKDL.Frame(T_L2_W.M)
swapped = False
if link1_idx > link2_idx:
link1_idx, link2_idx = link2_idx, link1_idx
swapped = True
if not (link1_idx, link2_idx) in link_collision_map:
link_collision_map[(link1_idx, link2_idx)] = []
for contact in report.contacts:
pos_W = PyKDL.Vector(contact.pos[0], contact.pos[1], contact.pos[2])
norm_W = PyKDL.Vector(contact.norm[0], contact.norm[1], contact.norm[2])
if swapped:
link_collision_map[(link1_idx, link2_idx)].append( (pos_W, -norm_W, T_L2_W * pos_W, T_L1_W * pos_W, TR_L2_W * (-norm_W), TR_L1_W * (-norm_W), contact.depth) )
else:
link_collision_map[(link1_idx, link2_idx)].append( (pos_W, norm_W, T_L1_W * pos_W, T_L2_W * pos_W, TR_L1_W * norm_W, TR_L2_W * norm_W, contact.depth) )
total_contacts += len(report.contacts)
col_chk.SetCollisionOptions(col_opt)
print "links in contact:", len(link_collision_map), "total contacts:", total_contacts
omega_col = np.matrix(np.zeros( (len(q),1) ))
Ncol = identityMatrix(len(q))
for link1_idx, link2_idx in link_collision_map:
link1_name = openrave.robot_rave.GetLinks()[link1_idx].GetName()
link2_name = openrave.robot_rave.GetLinks()[link2_idx].GetName()
# l1_parent = velma.fk_ik_solver.isParent(link1_name, link2_name)
# l2_parent = velma.fk_ik_solver.isParent(link2_name, link1_name)
affected_dof = velma.fk_ik_solver.getAffectedDof(link1_name, link2_name)
# print "affected dof:"
# for dof_idx in affected_dof:
# print q_names[dof_idx]
contacts = link_collision_map[ (link1_idx, link2_idx) ]
for c in contacts:
pos_W, norm_W, pos_L1, pos_L2, norm_L1, norm_L2, depth = c
m_id = self.pub_marker.publishVectorMarker(pos_W, pos_W + norm_W*0.05, 1, 1, 0, 0, frame='world', namespace='default', scale=0.005)
if depth < 0:
print "ERROR: depth < 0:", depth
exit(0)
# print link1_name, link2_name
jac1 = PyKDL.Jacobian(len(q))
velma.fk_ik_solver.getJacobianForX(jac1, link1_name, pos_L1, q, iq)
jac2 = PyKDL.Jacobian(len(q))
velma.fk_ik_solver.getJacobianForX(jac2, link2_name, pos_L2, q, iq)
# repulsive velocity
V_max = 0.1
depth_max = 0.002
if depth > depth_max:
depth = depth_max
Vrep = V_max * depth * depth / (depth_max * depth_max)
# the mapping between motions along contact normal and the Cartesian coordinates
e1 = norm_L1
e2 = norm_L2
Jd1 = np.matrix([e1[0], e1[1], e1[2]])
Jd2 = np.matrix([e2[0], e2[1], e2[2]])
# print "Jd1.shape", Jd1.shape
# rewrite the linear part of the jacobian
jac1_mx = np.matrix(np.zeros( (3, len(q)) ))
jac2_mx = np.matrix(np.zeros( (3, len(q)) ))
for q_idx in range(len(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]
# print "jac1_mx, jac2_mx"
# print jac1_mx
# print jac2_mx
# print "jac1_mx.shape", jac1_mx.shape
Jcol1 = Jd1 * jac1_mx
Jcol2 = Jd2 * jac2_mx
# print "Jcol2.shape", Jcol2.shape
Jcol = np.matrix(np.zeros( (2, len(q)) ))
for q_idx in range(len(q)):
if Jcol1[0, q_idx] < 0.000000001 or not q_idx in affected_dof:#l1_parent:
Jcol1[0, q_idx] = 0.0
if Jcol2[0, q_idx] < 0.000000001 or not q_idx in affected_dof:#l2_parent:
Jcol2[0, q_idx] = 0.0
Jcol[0, q_idx] = Jcol1[0, q_idx]
Jcol[1, q_idx] = Jcol2[0, q_idx]
# print Jcol.shape
# print "Jcol"
# print Jcol
Jcol_pinv = np.linalg.pinv(Jcol)
# Jcol_pinv = Jcol.transpose()
# print "Jcol_pinv"
# print Jcol_pinv
# Ncol1 = identityMatrix(len(q)) - np.linalg.pinv(Jcol1) * Jcol1
# Ncol2 = identityMatrix(len(q)) - np.linalg.pinv(Jcol2) * Jcol2
# Ncol = Ncol * Ncol1
# Ncol = Ncol * Ncol2
# omega_col += np.linalg.pinv(Jcol1) * (-Vrep)
# omega_col += np.linalg.pinv(Jcol2) * (Vrep)
# continue
# activation = min(1.0, depth/0.001)
# a_des = np.matrix(np.zeros( (len(q),len(q)) ))
# a_des[0,0] = a_des[1,1] = 1.0#activation
# U, S, V = numpy.linalg.svd(Jcol, full_matrices=True, compute_uv=True)
# print "V"
# print V
# print "S"
# print S
Ncol12 = identityMatrix(len(q)) - Jcol_pinv * Jcol
# Ncol12 = identityMatrix(len(q)) - Jcol.transpose() * (Jcol_pinv).transpose()
# Ncol12 = identityMatrix(len(q)) - (V * a_des * V.transpose())
# Ncol12 = identityMatrix(len(q)) - (Jcol.transpose() * a_des * Jcol)
Ncol = Ncol * Ncol12
Vrep1 = -Vrep
Vrep2 = Vrep
# if l1_parent:
# Vrep1 = 0.0
# if l2_parent:
# Vrep2 = 0.0
d_omega = Jcol_pinv * np.matrix([-Vrep, Vrep]).transpose()
# print "d_omega", d_omega
# print "Vrep", Vrep
# print q_names
# print "Jcol", Jcol
# print "Jcol_pinv", Jcol_pinv
# print "Jcol_pinv * Jcol", Jcol_pinv * Jcol
# print "Jcol * Jcol_pinv", Jcol * Jcol_pinv
# print "a_des", a_des
omega_col += d_omega
# print "depth", depth
# raw_input(".")
# print "omega_col", omega_col
# print dx_HAND_ref
omega_r_HAND = (J_r_HAND_inv * np.matrix(dx_r_HAND_ref).transpose())
omega_l_HAND = (J_l_HAND_inv * np.matrix(dx_l_HAND_ref).transpose())
Ncol_inv = np.linalg.pinv(Ncol)
N_r_HAND_inv = np.linalg.pinv(N_r_HAND)
# omega = J_JLC_inv * np.matrix(dx_JLC_ref).transpose() + N_JLC_inv * (omega_col + Ncol_inv * (omega_r_HAND))# + N_r_HAND_inv * omega_l_HAND))
omega = J_JLC_inv * np.matrix(dx_JLC_ref).transpose() + N_JLC.transpose() * (omega_col + Ncol.transpose() * (omega_r_HAND))# + N_r_HAND.transpose() * omega_l_HAND))
# print "omega", omega
# print "dx_JLC_ref"
# print dx_JLC_ref
# print "dx_HAND_ref"
# print dx_HAND_ref
omega_vector = np.empty(len(q))
for q_idx in range(len(q)):
omega_vector[q_idx] = omega[q_idx][0]
q += omega_vector * 0.002
if time_elapsed.to_sec() > 0.2:
last_time = rospy.Time.now()
velma.moveJoint(q, velma.getJointStatesVectorNames(), 0.05, start_time=0.14)
