def getJacobian(self, base_name, end_name, q):
if not (base_name, end_name) in self.fk_solvers:
self.fk_solvers[(base_name, end_name)] = FkIkSolver.FkSolver(
self.tree, base_name, end_name, self.joint_names_vector)
fk_solver = self.fk_solvers[(base_name, end_name)]
# extract joint values for the chain
q_jac = PyKDL.JntArray(fk_solver.chain_length)
q_jac_idx = 0
for q_idx in fk_solver.q_indices_map:
q_jac[q_jac_idx] = q[q_idx]
q_jac_idx += 1
jac_small = PyKDL.Jacobian(fk_solver.chain.getNrOfJoints())
fk_solver.jac_solver.JntToJac(q_jac, jac_small)
# create the jacobian for all joints
jac_big = np.matrix(np.zeros((6, len(q))))
for col_idx in range(jac_small.columns()):
q_idx = fk_solver.q_indices_map[col_idx]
col = jac_small.getColumn(col_idx)
for row_idx in range(6):
jac_big[row_idx, q_idx] = col[row_idx]
return jac_big
def __init__(self, urdf, world='world', tip='tip'):
# Try loading the URDF data into a KDL tree.
(ok, self.tree) = kdlp.treeFromString(urdf)
assert ok, "Unable to parse the URDF"
# Save the base and tip frame names.
self.baseframe = world
self.tipframe = tip
# Create the isolated chain from world to tip.
self.chain = self.tree.getChain(world, tip)
# Extract the number of joints.
self.N = self.chain.getNrOfJoints()
assert self.N > 0, "Found no joints in the chain"
# Create storage for the joint position, tip position, and
# Jacobian matrices (for the KDL library).
self.qkdl = kdl.JntArray(self.N)
self.Tkdl = kdl.Frame()
self.Jkdl = kdl.Jacobian(self.N)
# Also pre-allocate the memory for the numpy return variables.
# The content will be overwritten so the initial values are
# irrelevant.
self.p = np.zeros((3, 1))
self.R = np.identity(3)
self.J = np.zeros((6, self.N))
# Instantiate the solvers for tip position and Jacobian.
self.fkinsolver = kdl.ChainFkSolverPos_recursive(self.chain)
self.jacsolver = kdl.ChainJntToJacSolver(self.chain)
def jacobian(self, joint_values=None):
"""Compute the Jacobian for the current joint positions."""
if joint_values is None:
joint_values = self.joint_angles
jac = PyKDL.Jacobian(self.n_joints)
self._jac_kdl.JntToJac(self.joints_to_kdl('positions', joint_values), jac)
return self.kdl_to_mat(jac)
def jacobian(self, joint_values=None):
"""Compute the Jacobian for the current joint positions."""
jnt_array = self.joints_to_kdl('positions')
jac = PyKDL.Jacobian(jnt_array.rows())
self._jac_kdl.JntToJac(jnt_array, jac)
mat = self.kdl_to_mat(jac)
return mat
def update_joints(self, joint_msg):
for i, n in enumerate(self.joint_names):
index = joint_msg.name.index(n)
self.joints[i] = joint_msg.position[index]
if self.debug:
self.debug_count += 1
if (self.debug_count % 10) == 0:
frame = kdl.Frame()
self.fk_ee.JntToCart(self.joints, frame)
print " "
print " "
print "frame translation:"
print frame.p
print "frame rotation:"
print frame.M
print " "
jacobian = kdl.Jacobian(self.num_joints)
self.jac_ee.JntToJac(self.joints, jacobian)
print "jacobian"
print jacobian
def get_jacobian(self, d_chain):
# Obtain jacobian for the selected arm
if d_chain == 'left_chain':
jacobian = kdl.Jacobian(self.left_chain.getNrOfJoints())
self.jac_solver_left.JntToJac(self.left_jnt_pos, jacobian)
# print '\n Left: \n', jacobian
elif d_chain == 'right_chain':
jacobian = kdl.Jacobian(self.right_chain.getNrOfJoints())
self.jac_solver_right.JntToJac(self.right_jnt_pos, jacobian)
# print '\n Right \n', jacobian
else:
rospy.logerr('Wrong chain specified for Jacobian')
jacobian = kdl.Jacobian(self.chain.getNrOfJoints())
return jacobian
def jacobian(self, joint_values):
jacobian = PyKDL.Jacobian(self.NUM_JOINTS)
joint_array = PyKDL.JntArray(self.NUM_JOINTS)
for i, val in enumerate(joint_values):
joint_array[i] = val
self.jac_solver.JntToJac(joint_array, jacobian)
return self.kdl_to_mat(jacobian)
def jacobian(self):
'''
Calculates jacobian based on joint values - argument to pass ( to do )
'''
joint_values = self.phi
jacobian = PyKDL.Jacobian(self.num_joints)
self.jac_kdl.JntToJac(self.joints_to_kdl(joint_values),
jacobian) #changes jacobian to self.jac_matrix
temp = jacobian
return self.kdl_to_mat(temp)
def jacobian(self, q, pos=None):
j_kdl = kdl.Jacobian(self.num_joints)
q_kdl = joint_list_to_kdl(q)
self._jac_kdl.JntToJac(q_kdl, j_kdl)
if pos is not None:
ee_pos = self.forward(q)[:3, 3]
pos_kdl = kdl.Vector(pos[0] - ee_pos[0], pos[1] - ee_pos[1],
pos[2] - ee_pos[2])
j_kdl.changeRefPoint(pos_kdl)
return kdl_to_mat(j_kdl)
def jacobian(self, joint_values=None, end_link=None):
"""Compute the Jacobian for the current joint positions."""
if end_link is not None:
jnt_array = self.joints_to_kdl(
'positions',
last_joint=self.joint_names[self.link_names.index(end_link)])
jac = PyKDL.Jacobian(jnt_array.rows())
jac_kdl_part = self._jac_kdl_part[end_link]
jac_kdl_part.JntToJac(jnt_array, jac)
mat = self.kdl_to_mat(jac)
mat_add = np.matrix(
np.zeros((mat.shape[0], mat.shape[0] - mat.shape[1])))
mat = np.concatenate((mat, mat_add), axis=1)
else:
jnt_array = self.joints_to_kdl('positions')
jac = PyKDL.Jacobian(jnt_array.rows())
self._jac_kdl.JntToJac(jnt_array, jac)
mat = self.kdl_to_mat(jac)
return mat
def get_jacobian(self):
# Obtain jacobian for the selected arm
self.jac_solver = kdl.ChainJntToJacSolver(self.chain)
jacobian = kdl.Jacobian(self.nJoints)
self.jac_solver.JntToJac(self.joint_values_kdl, jacobian)
jac_array = np.empty(0)
for row in range(jacobian.rows()):
for col in range(jacobian.columns()):
jac_array = np.hstack((jac_array, jacobian[row, col]))
jac_array = np.reshape(jac_array,
(jacobian.rows(), jacobian.columns()))
return jac_array
def get_jacobian(self):
#initialize seed array from current joint states
seed_array = PyKDL.JntArray(len(self._joint_positions))
for idx, val in enumerate(self._joint_positions):
seed_array[idx] = val
#compute the current jacobian and convert it into numpy array
jacobian = PyKDL.Jacobian(len(self._joint_positions))
self._jac_kdl.JntToJac(seed_array, jacobian)
J = np.zeros([int(jacobian.rows()), int(jacobian.columns())])
for i in range(int(jacobian.rows())):
for j in range(int(jacobian.columns())):
J[i, j] = jacobian[i, j]
return J
def Jac_kdl(self,arm,q):
''' returns the Jacobian, given the joint angles
'''
J_kdl = kdl.Jacobian(7)
self.cody_kdl[arm]['jacobian_solver'].JntToJac(q,J_kdl)
kdl_jac = np.matrix([
[J_kdl[0,0],J_kdl[0,1],J_kdl[0,2],J_kdl[0,3],J_kdl[0,4],J_kdl[0,5],J_kdl[0,6]],
[J_kdl[1,0],J_kdl[1,1],J_kdl[1,2],J_kdl[1,3],J_kdl[1,4],J_kdl[1,5],J_kdl[1,6]],
[J_kdl[2,0],J_kdl[2,1],J_kdl[2,2],J_kdl[2,3],J_kdl[2,4],J_kdl[2,5],J_kdl[2,6]],
[J_kdl[3,0],J_kdl[3,1],J_kdl[3,2],J_kdl[3,3],J_kdl[3,4],J_kdl[3,5],J_kdl[3,6]],
[J_kdl[4,0],J_kdl[4,1],J_kdl[4,2],J_kdl[4,3],J_kdl[4,4],J_kdl[4,5],J_kdl[4,6]],
[J_kdl[5,0],J_kdl[5,1],J_kdl[5,2],J_kdl[5,3],J_kdl[5,4],J_kdl[5,5],J_kdl[5,6]],
])
return kdl_jac
def Jac_kdl(self, arm, q):
J_kdl = kdl.Jacobian(7)
if arm != 0:
rospy.logerr('Unsupported arm: '+ str(arm))
return None
self.right_jac.JntToJac(q,J_kdl)
kdl_jac = np.matrix([
[J_kdl[0,0],J_kdl[0,1],J_kdl[0,2],J_kdl[0,3],J_kdl[0,4],J_kdl[0,5],J_kdl[0,6]],
[J_kdl[1,0],J_kdl[1,1],J_kdl[1,2],J_kdl[1,3],J_kdl[1,4],J_kdl[1,5],J_kdl[1,6]],
[J_kdl[2,0],J_kdl[2,1],J_kdl[2,2],J_kdl[2,3],J_kdl[2,4],J_kdl[2,5],J_kdl[2,6]],
[J_kdl[3,0],J_kdl[3,1],J_kdl[3,2],J_kdl[3,3],J_kdl[3,4],J_kdl[3,5],J_kdl[3,6]],
[J_kdl[4,0],J_kdl[4,1],J_kdl[4,2],J_kdl[4,3],J_kdl[4,4],J_kdl[4,5],J_kdl[4,6]],
[J_kdl[5,0],J_kdl[5,1],J_kdl[5,2],J_kdl[5,3],J_kdl[5,4],J_kdl[5,5],J_kdl[5,6]],
])
return kdl_jac
def force_to_torques(self, forces, joint_positions):
# TODO test (correct reference frame?????)
torques = []
i = 0
for f, j in zip(forces, joint_positions):
jacobian = kdl.Jacobian(3)
joints = kdl.JntArray(3)
joints[0] = joint_positions[i + 0]
joints[1] = joint_positions[i + 1]
joints[2] = joint_positions[i + 2]
self.jac_list[i].JntToJac(joints, jacobian)
jacobian = rm.jac_to_np(jacobian)
f.reshape(3, 1)
f = np.vstack([f.reshape(3, 1), np.zeros((3, 1))])
t = (jacobian.T).dot(f)
torques.append(t)
i += 1
return torques
def get_jacobian(self, joint_angles):
"""
Return the geometric jacobian on the given joint angles.
Refer to P112 in "Robotics: Modeling, Planning, and Control"
:param joint_angles: joint_angles
:type joint_angles: list or flattened np.ndarray
:return: jacobian
:rtype: np.ndarray
"""
q = kdl.JntArray(self.urdf_chain.getNrOfJoints())
for i in range(q.rows()):
q[i] = joint_angles[i]
jac = kdl.Jacobian(self.urdf_chain.getNrOfJoints())
fg = self.jac_solver.JntToJac(q, jac)
assert fg == 0, 'KDL JntToJac error!'
jac_np = prutil.kdl_array_to_numpy(jac)
return jac_np
def get_jacobian(self, joint_angles):
"""
Return the geometric jacobian on the given joint angles.
Refer to P112 in "Robotics: Modeling, Planning, and Control".
Args:
joint_angles (list or flattened np.ndarray): joint angles.
Returns:
np.ndarray: jacobian (shape: :math:`[6, DOF]`).
"""
q = kdl.JntArray(self._urdf_chain.getNrOfJoints())
for i in range(q.rows()):
q[i] = joint_angles[i]
jac = kdl.Jacobian(self._urdf_chain.getNrOfJoints())
fg = self._jac_solver.JntToJac(q, jac)
if fg < 0:
raise ValueError('KDL JntToJac error!')
jac_np = kdl_array_to_numpy(jac)
return jac_np
def __init__(self, urdf, world='world', tip='tip'):
# Try loading the URDF data into a KDL tree.
(ok, self.tree) = kdlp.treeFromString(urdf)
assert ok, "Unable to parse the URDF"
# Create the isolated chain from world to tip.
self.chain = self.tree.getChain(world, tip)
# Extract the number of joints.
self.N = self.chain.getNrOfJoints()
assert self.N > 0, "Found no joints in the chain"
# Create storage for the joint position, tip position, and
# Jacobian matrices (for the KDL library).
self.qkdl = kdl.JntArray(self.N)
self.Tkdl = kdl.Frame()
self.Jkdl = kdl.Jacobian(self.N)
# Instantiate the solvers for tip position and Jacobian.
self.fkinsolver = kdl.ChainFkSolverPos_recursive(self.chain)
self.jacsolver = kdl.ChainJntToJacSolver(self.chain)
def force_to_torques(self, forces, joint_positions):
# TODO test (correct reference frame?????)
torques = []
for i in range(4):
# print(f)
jacobian = kdl.Jacobian(3)
joints = kdl.JntArray(3)
joints[0] = joint_positions[3 * i + 0]
joints[1] = joint_positions[3 * i + 1]
joints[2] = joint_positions[3 * i + 2]
# print(joints)
self.jac_list[i].JntToJac(joints, jacobian)
jacobian = rm.jac_to_np(jacobian)
f = np.array(
[forces[3 * i + 0], forces[3 * i + 1], forces[3 * i + 2]])
f = np.vstack([f.reshape(3, 1), np.zeros((3, 1))])
# flip torque, want let to apply downwards force
t = (jacobian.T).dot(-f)
torques.append(t[:, 0])
i += 1
return np.concatenate(torques)
def __init__(self, robotDescriptionString=None, arm_id='panda'):
if robotDescriptionString is None:
self.urdf_string = _rospy.get_param('robot_description')
self.arm_id = _rospy.get_param('arm_id')
else:
self.urdf_string = robotDescriptionString
self.arm_id = arm_id
self.baseLinkName = "{}_link0".format(self.arm_id)
self.eeLinkName = "{}_link7".format(self.arm_id)
isSuccessful, self.kdltree = _kdl_parser.treeFromString(
self.urdf_string)
if not isSuccessful:
raise RuntimeError("could not parse 'robot_description'")
self.ee_chain = self.kdltree.getChain(self.baseLinkName,
self.eeLinkName)
self.fk_ee = _kdl.ChainFkSolverPos_recursive(self.ee_chain)
self.jointposition = _kdl.JntArray(7)
self.jointvelocity = _kdl.JntArray(7)
self.eeFrame = _kdl.Frame()
# Calculate the jacobian expressed in the base frame of the chain, with reference point at the end effector of the *chain.
# http://docs.ros.org/hydro/api/orocos_kdl/html/classKDL_1_1ChainJntToJacSolver.html#details
# Sounds like base jacobian but will be used here for both
self.jac_ee = _kdl.ChainJntToJacSolver(self.ee_chain)
self.jacobian = _kdl.Jacobian(7)
#dynamics: (needs masses added to urdf!)
self.grav_vector = _kdl.Vector(0., 0., -9.81)
self.dynParam = _kdl.ChainDynParam(self.ee_chain, self.grav_vector)
self.inertiaMatrix_kdl = _kdl.JntSpaceInertiaMatrix(7)
self.inertiaMatrix = _np.eye((8))
self.gravity_torques_kdl = _kdl.JntArray(7)
self.gravity_torques = _np.zeros((8))
viscuousFriction = [1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5,
1.5] #TODO: load from URDF
self.viscuousFriction = _np.array(viscuousFriction)
def compute_jacobian(self, joint_angles_kdl_array):
'''Function to compute the Jacobian at a particular set of joint angles'''
jacobian = PyKDL.Jacobian(self.num_joints)
self.jacobian_solver.JntToJac(joint_angles_kdl_array, jacobian)
jacobian_matrix = self.kdl_array_to_numpy_mat(jacobian)
return jacobian_matrix
def jacobian(self):
jacobian = PyKDL.Jacobian(self._num_jnts)
self._jac_kdl.JntToJac(self.joints_to_kdl('positions'), jacobian)
return self.kdl_to_mat(jacobian)
def get_jacobian(self, joint):
jac = PyKDL.Jacobian(self.kine_chain.getNrOfJoints())
self.jac_calc.JntToJac(joint, jac)
return jac
def __init__(self, freq_control=100, margin_workspace=0.05):
# Load robot
urdf_model = URDF.from_parameter_server()
fetch = kdl_tree_from_urdf_model(urdf_model)
fetch_arm = fetch.getChain(BASE_LINK, END_LINK)
self.dof = fetch_arm.getNrOfJoints()
self.kdl_pos = PyKDL.ChainFkSolverPos_recursive(fetch_arm)
self.kdl_jac = PyKDL.ChainJntToJacSolver(fetch_arm)
self.kdl_dyn = PyKDL.ChainDynParam(fetch_arm,
PyKDL.Vector(0, 0, -9.81))
self.kdl_q = PyKDL.JntArray(self.dof)
self.kdl_A = PyKDL.JntSpaceInertiaMatrix(self.dof)
self.kdl_J = PyKDL.Jacobian(self.dof)
self.kdl_x = PyKDL.Frame()
# self.kdl_G = PyKDL.JntArray(self.dof)
# self.G = np.zeros((self.dof,))
# Initialize robot values
self.lock = threading.Lock()
self.thread_q = np.zeros((self.dof, ))
self.thread_dq = np.zeros((self.dof, ))
self.thread_tau = np.zeros((self.dof, ))
self.q = np.zeros((self.dof, ))
self.dq = np.zeros((self.dof, ))
self.tau = np.zeros((self.dof, ))
self.x = np.zeros((3, ))
self.quat = np.array([0., 0., 0., 1.])
self.lim_norm_x = self.compute_workspace() - margin_workspace
self.A = np.zeros((self.dof, self.dof))
self.A_inv = np.zeros((self.dof, self.dof))
self.J = np.zeros((6, self.dof))
self.q_init = np.array([
0.,
-np.pi / 4.,
0.,
np.pi / 4,
0.,
np.pi / 2,
0.,
]) # Face down
self.q_tuck = np.array([1.32, 1.40, -0.2, 1.72, 0.0, 1.66,
0.0]) # Storage position
self.q_stow = np.array([1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0])
self.q_intermediate_stow = np.array(
[0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0])
self.x_des = np.array([0.8, 0., 0.35]) # q_init
self.x_init = np.array([0.8, 0., 0.35]) # q_init
# self.quat_des = np.array([-0.707, 0., 0.707, 0.]) # Face up
self.quat_des = np.array([0., 0.707, 0., 0.707]) # Face down
self.state = "INITIALIZE"
print("Switching to state: " + self.state)
self.t_start = time.time()
self.freq_control = freq_control
# Initialize pub and sub
self.pub = rospy.Publisher("arm_controller/joint_torque/command",
Float64MultiArray,
queue_size=1)
sub = rospy.Subscriber(
"joint_states", JointState,
lambda joint_states: self.read_joint_sensors(joint_states))
# Initialize ROS
rospy.init_node("joint_space_controller")
def __init__(self, chain):
self.chain = chain
self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain)
self.jac_solver = PyKDL.ChainJntToJacSolver(self.chain)
self.jacobian = PyKDL.Jacobian(self.chain.getNrOfJoints())
self.joints = self.get_joints()
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")
#求取坐标的逆 frame_inv = f3.Inverse() print "frame_inv:\n", frame_inv #create a wrench wr = PyKDL.Wrench() wr.force = PyKDL.Vector(0, 1, 2) wr.torque = PyKDL.Vector(3, 4, 5) print "wr:\n", wr #read a wrench f = wr.force print "f:", f M = wr.torque print "M:", M #create a Twist Tw = PyKDL.Twist() Tw.vel = PyKDL.Vector(0, 1, 2) Tw.rot = PyKDL.Vector(3, 4, 5) print "Tw:\n", Tw #read a Twist v = Tw.vel print "v:", v w = Tw.rot print "w:", w #create a jacobian jac = PyKDL.Jacobian(7) print "jac", jac.columns()
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 jacobian(self,joint_values=None):
jacobian = PyKDL.Jacobian(self._num_jnts)
self._jac_kdl.JntToJac(self.joints_to_kdl('positions',joint_values), jacobian)
return kdl_to_mat(jacobian)
def compute_jacobian(self, joint_angles_kdl_array):
jacobian = PyKDL.Jacobian(self.num_joints)
self.jacobian_solver.JntToJac(joint_angles_kdl_array, jacobian)
jacobian_matrix = self.kdl_array_to_numpy_mat(jacobian)
return jacobian_matrix
