def __init__(self,
urdfRootPath=pybullet_data.getDataPath(),
timeStep=0.01):
self.urdfRootPath = urdfRootPath
self.timeStep = timeStep
self.maxVelocity = .35
self.maxForce = 200.
self.fingerAForce = 2
self.fingerBForce = 2.5
self.fingerTipForce = 2
self.useSimulation = 1
self.useNullSpace = 21
self.useOrientation = 1
self.handId = 3
self.kukaEndEffectorIndex = 6
self.kukaGripperIndex = 7
self.jointInfo = JointInfo()
self.fingerTip_joint_name = [
"J1_FF", "J1_MF", "J1_RF", "J1_LF", "THJ1"
] #this are joints between final link and one before it
self.wrist_joint_name = ["WRJ2", "WRJ1"]
#lower limits for null space
self.ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space
self.ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
#joint ranges for null space
self.jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space
self.rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents
self.jd = [
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001
]
self.reset()
def __init__(self, target_xyz, initial_r, target_dim, hand_length,
grownth_increment):
self.target_dim = target_dim
self.initial_r = initial_r
self.hand_length = hand_length
self.target_xyz = target_xyz
self.radius = self.set_initial_r()
self.grownth_increment = grownth_increment
self.jointInfo = JointInfo()
self.kuka_handId = 3
def __init__(self,
ee_xyz,
kuka_loc=None,
urdfRootPath=pybullet_data.getDataPath(),
timeStep=0.01):
self.kuka_loc = kuka_loc
self.urdfRootPath = urdfRootPath
self.timeStep = timeStep
self.maxVelocity = .35
self.maxForce = 200.
self.fingerAForce = 2
self.fingerBForce = 2.5
self.fingerTipForce = 2
self.useSimulation = 1
self.useNullSpace = 21
self.useOrientation = 1
self.kuka_handId = 3
self.kukaEndEffectorIndex = 6
self.kukaGripperIndex = 7
self.ee_xyz = ee_xyz
self.jointInfo = JointInfo()
model_info_path = resource_filename(
__name__, "/kuka_handlit_model/model_info.yml")
self.modelInfo = ModelInfo(model_info_path)
self.fingerTip_link_name = [
"distal_FF", "distal_MF", "distal_RF", "thdistal"
] #this are joints between final link and one before it
#lower limits for null space
self.ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space
self.ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
#joint ranges for null space
self.jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space
self.rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents
self.jd = [
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001
]
self.reset()
def setup_GUI_slidbars(Robot, RobotId):
ids = []
#Getting robot info
jointInfo = JointInfo()
jointInfo.get_infoForAll_joints(Robot)
#getting number of active joints
active_joints_info = jointInfo.getActiveJointsInfo()
num_active_joints = jointInfo.getNumberOfActiveJoints()
#setting up gui slider for each active joint
for i in range(num_active_joints):
jointName = active_joints_info[i]["jointName"]
jointIndex = active_joints_info[i]["jointIndex"]
jointll = active_joints_info[i]["jointLowerLimit"]
jointul = active_joints_info[i]["jointUpperLimit"]
jointPositionState = p.getJointState(RobotId, jointIndex)[0]
sliderID = p.addUserDebugParameter(jointName, jointll, jointul,
jointPositionState)
ids.append(sliderID)
return ids
p.connect(p.GUI)
finger = p.loadSDF("./model.sdf")
fingerID = finger[0]
p.resetBasePositionAndOrientation(fingerID,[0.00000,0.200000,0.65000],[0.000000,0.000000,0.000000,1.000000])
table = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"table/table.urdf"), 0.5000000,0.60000,0.0000,0.000000,0.000000,0.0,1.0)
texUid = p.loadTexture("./cube_new/aaa.png")
cube_objects = p.loadSDF("./cube_new/model.sdf")
cubeId = cube_objects[0]
p.changeVisualShape(cube_objects[0], -1,rgbaColor =[1,1,1,1])
p.changeVisualShape(cube_objects[0], -1, textureUniqueId = texUid)
p.resetBasePositionAndOrientation(cube_objects[0],(0.5000000,0.60000,0.6700),(0.717,0.0,0.0,0.717))
#blockUid = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"block.urdf"), xpos,ypos,zpos,orn[0],orn[1],orn[2],orn[3])
blockUid = 2
p.loadURDF(os.path.join(pybullet_data.getDataPath(),"plane.urdf"),[0,0,0])
jointInfo = JointInfo()
jointInfo.get_infoForAll_joints(finger)
active_joints_info = jointInfo.getActiveJointsInfo()
num_active_joints = jointInfo.getNumberOfActiveJoints()
#print("active_joints_info::",active_joints_info)
#print("finger::",finger)
num_joints = p.getNumJoints(fingerID)
#print("`num of joints:::",num_joints)
for i in range(num_joints-1):
j_info = p.getJointInfo(fingerID,i)
#print("joint_info::",j_info)
if index_collision_set_2 == active_joints_info[j]["jointIndex"]:
index_collision_set_2_result = j
collision_result.append([
active_joints_info[index_collision_set_1_result]["linkName"],
active_joints_info[index_collision_set_2_result]["linkName"]
])
return collision_result
p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
finger = p.loadSDF("./model.sdf")
fingerID = finger[0]
jointInfo = JointInfo()
jointInfo.get_infoForAll_joints(finger)
active_joints_info = jointInfo.getActiveJointsInfo()
num_active_joints = jointInfo.getNumberOfActiveJoints()
num_joints = p.getNumJoints(fingerID)
# print("active_joints_info::",active_joints_info)
# print("finger::",finger)
# print("`num of joints:::",num_joints)
"""
for i in range(num_joints):
j_info = p.getJointInfo(fingerID,i)
print("joint_info::",j_info)
"""
# texUid = p.loadTexture("./../cube_new/aaa.png")
from pkg_resources import parse_version
import os, inspect
currentdir = os.path.dirname(
os.path.abspath(inspect.getfile(inspect.currentframe())))
# print ("current_dir=" + currentdir)
os.sys.path.insert(0, currentdir)
from mamad_util import JointInfo
modelInfo = ModelInfo(path="./model_info.yml")
p.connect(p.GUI)
finger = p.loadSDF("./model.sdf")
fingerID = finger[0]
p.resetBasePositionAndOrientation(fingerID, [0.00000, 0.200000, 0.65000],
[0.000000, 0.000000, 0.000000, 1.000000])
jointInfo = JointInfo()
jointInfo.get_infoForAll_joints(finger)
active_joints_info = jointInfo.getActiveJointsInfo()
num_active_joints = jointInfo.getNumberOfActiveJoints()
xpos = 0.05
ypos = 0.04
zpos = 1
ang = 3.14 * 0.5
orn = p.getQuaternionFromEuler([0, 0, ang])
table = p.loadURDF(
os.path.join(pybullet_data.getDataPath(), "table/table.urdf"), 0.5000000,
0.60000, 0.0000, 0.000000, 0.000000, 0.0, 1.0)
texUid = p.loadTexture("./cube_new/aaa.png")
cube_objects = p.loadSDF("./cube_new/model.sdf")
cubeId = cube_objects[0]
p.changeVisualShape(cube_objects[0], -1, rgbaColor=[1, 1, 1, 1])
class Hand:
def __init__(self,
urdfRootPath=pybullet_data.getDataPath(),
timeStep=0.01):
self.urdfRootPath = urdfRootPath
self.timeStep = timeStep
self.maxVelocity = .35
self.maxForce = 200.
self.fingerAForce = 2
self.fingerBForce = 2.5
self.fingerTipForce = 2
self.useSimulation = 1
self.useNullSpace = 21
self.useOrientation = 1
self.handId = 3
self.kukaEndEffectorIndex = 6
self.kukaGripperIndex = 7
self.jointInfo = JointInfo()
self.fingerTip_joint_name = [
"J1_FF", "J1_MF", "J1_RF", "J1_LF", "THJ1"
] #this are joints between final link and one before it
self.wrist_joint_name = ["WRJ2", "WRJ1"]
#lower limits for null space
self.ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space
self.ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
#joint ranges for null space
self.jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space
self.rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents
self.jd = [
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001
]
self.reset()
def get_fingerTips_linkIndex(self):
fingerTips_linkIndex = []
fingerTips_jointInfo = []
fingerTip_joint_name_bytes = [
i.encode(encoding='UTF-8', errors='strict')
for i in self.fingerTip_joint_name
]
#print("Debug11::fingerTip_joint_name_bytes",fingerTip_joint_name_bytes)
# getting joints for the final link
# print(fingerTip_joint_name_bytes)
for i in fingerTip_joint_name_bytes:
# print(i," ",self.jointInfo.searchBy(key="jointName",value = i))
fingerTips_jointInfo.append(
self.jointInfo.searchBy(key="jointName", value=i))
#extracting joint index which equivalent to link index
# print("Debug13::len(fingerTips_jointInfo)",len(fingerTips_jointInfo))
for i in fingerTips_jointInfo:
i = i[0]
#print("Debug11 i",i)
#print("Debug12::i['jointIndex']",i["jointIndex"])
fingerTips_linkIndex.append(i["jointIndex"])
#print("Debug14::fingerTips_linkIndex",fingerTips_linkIndex)
return fingerTips_linkIndex
def get_wrist_index(self):
wrist_index = []
wrist_jointinfo = []
#wrist_joint_name_bytes = [i.encode(encoding='UTF-8',errors='strict') for i in self.wrist_joint_name]
for i in self.wrist_joint_name:
wrist_jointinfo.append(
self.jointInfo.searchBy(key="jointName", value=i))
for i in wrist_jointinfo:
i = i[0]
wrist_index.append(i["jointIndex"])
#print("wrist index",wrist_index)
return wrist_index
# def check_wrist_action_complete(self,motorCommands):
# wrist_state = []
# wrist_index = self.get_wrist_index()
# setcommand = []
# sumerror = 0
# for i in wrist_index:
# wrist_pos = p.getJointState(self.handId,i)[0]
# wrist_state.append(wrist_pos)
# setcommand.append(motorCommands[i])
# #sumerror += abs(setcommand[i]-wrist_state[i])
# print("wrist state",wrist_state)
# #print("error",sumerror)
# print("set",setcommand)
# if sumerror <= 1e-5:
# return True
# else:
# return False
def cameraSetup(self):
#https://github.com/bulletphysics/bullet3/issues/1616
width = 128
height = 128
fov = 60 # field of view
aspect = width / height
near = 0.02
far = 1
endEffector_info = p.getLinkState(self.handId,
self.kukaEndEffectorIndex,
computeForwardKinematics=True)
# print("endEffector_info",endEffector_info)
endEffector_pos = endEffector_info[4]
endEffector_ori = endEffector_info[5]
# print("endEffector_pos",endEffector_pos)
# print("endEffector_ori",endEffector_ori)
endEffector_pos_Xoffset = 0.
endEffector_pos_Zoffset = -0.05
endEffector_pos_InWorldPosition = endEffector_pos
cameraEyePosition = endEffector_pos_InWorldPosition
cameraEyePosition_ = [
endEffector_pos[0] + endEffector_pos_Xoffset, endEffector_pos[1],
endEffector_pos[2] + endEffector_pos_Zoffset
]
rot_matrix = p.getMatrixFromQuaternion(endEffector_ori)
rot_matrix = np.array(rot_matrix).reshape(3, 3)
# Initial vectors
init_camera_vector = (0, 0, 1) # z-axis
init_up_vector = (0, 1, 0) # y-axis
# Rotated vectors
camera_vector = rot_matrix.dot(init_camera_vector)
up_vector = rot_matrix.dot(init_up_vector)
cameraEyePosition = endEffector_pos_InWorldPosition
dist_cameraTargetPosition = -0.02
view_matrix = p.computeViewMatrix(
cameraEyePosition_, cameraEyePosition + 0.1 * camera_vector,
up_vector)
projection_matrix = p.computeProjectionMatrixFOV(
fov, aspect, near, far)
# Get depth values using the OpenGL renderer
images = p.getCameraImage(width,
height,
view_matrix,
projection_matrix,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_opengl = np.reshape(images[2], (height, width, 4)) * 1. / 255.
depth_buffer_opengl = np.reshape(images[3], [width, height])
depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl)
seg_opengl = np.reshape(images[4], [width, height]) * 1. / 255.
time.sleep(1)
def reset(self):
dirpath = os.getcwd()
# self._hand = p.loadSDF(dirpath+"/gym_test/envs/shadow_hand_vijay/model.sdf")
hand_path = resource_filename(__name__,
"./shadow_hand_vijay/model.sdf")
self._hand = p.loadSDF(hand_path)
self.handId = self._hand[0]
#self.cameraSetup()
# reset joints to the middle valu
#p.resetBasePositionAndOrientation(self.handId,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
p.resetBasePositionAndOrientation(
self.handId, [0.000, 0.000, 0.000],
[0.7071, 0.000000, 0.000000, -0.7071])
self.jointInfo.get_infoForAll_joints(self._hand)
self.numJoints = p.getNumJoints(self.handId)
self.num_Active_joint = self.jointInfo.getNumberOfActiveJoints()
#print(" self.num_Active_join:::", self.num_Active_joint)
self.indexOf_activeJoints = self.jointInfo.getIndexOfActiveJoints()
#reseting to the middle
#resetting to the upper limit
# print("Debug::self.indexOf_activeJoints",self.indexOf_activeJoints)
self.trayUid = p.loadURDF(
os.path.join(self.urdfRootPath, "tray/tray.urdf"), 0.640000,
0.075000, -0.190000, 0.000000, 0.000000, 1.000000, 0.000000)
self.endEffectorPos = [0.537, 0.0, 0.5]
self.endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
for i in range(self.numJoints):
jointInfo = p.getJointInfo(self.handId, i)
qIndex = jointInfo[3]
if qIndex > -1:
#print("motorname")
#print(jointInfo[1])
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
def getActionDimension(self):
numOf_activeJoints = self.jointInfo.numberOfActiveJoints()
return numOf_activeJoints #position x,y,z and roll/pitch/yaw euler angles of end effector
def getObservationDimension(self):
return len(self.getObservation())
def getObservation_joint(self, format="list"):
indexOfActiveJoints = self.jointInfo.getIndexOfActiveJoints()
jointsInfo = self.jointInfo.getActiveJointsInfo()
jointsStates = []
joints_state = {} #key:joint ,value = joint state
for i in range(len(indexOfActiveJoints)):
jointName = jointsInfo[i]["jointName"]
jointIndex = indexOfActiveJoints[i]
jointState = p.getJointState(self._hand[0], jointIndex)
joints_state[jointName] = jointState[0]
jointsStates.append(jointState[0])
if format == "dictinary":
return joints_state
else:
return jointsStates
def getObservation(self):
#self.cameraSetup()
state_dict = {}
observation = []
fingerTipIndexs = self.get_fingerTips_linkIndex()
#print("Debug::fingerTipIndexs",fingerTipIndexs)
counter = 0
#getting fingers tip position and orientation
for index in fingerTipIndexs:
state = p.getLinkState(
self.handId,
index) #mamad:returns endeffector info position orientation
pos = state[0] #mamad: linkWorldPosition
orn = state[1] #mamad: linkWorldOrientation
state_dict[self.fingerTip_joint_name[counter]] = {
"pos": pos,
"orn": orn
}
counter += 1
#print("Debug::state_dict",state_dict)
for finger in self.fingerTip_joint_name:
euler = p.getEulerFromQuaternion(state_dict[finger]["orn"])
pos = state_dict[finger]["pos"]
observation.extend(list(pos))
# observation.extend(list(euler))
#print("Debug::observation",observation)
#print("Debug::len(observation)",len(observation))
return observation
def applyAction(self, motorCommands):
#The actions are going to Active joint values.
#gettting current state of Avtive joints before applying actions This is different that the state we get in getObservation
joint_state = [] #current joint postion
new_joint_pos = [
0
] * self.jointInfo.getNumberOfActiveJoints() # new joint position
for jointIndex in self.indexOf_activeJoints:
joint_pos = p.getJointState(self.handId, jointIndex)[0]
joint_state.append(joint_pos)
# print("Debug::joint_state",joint_state)
#making sure the joint values suggested by agent does not exceed joint limit
#design question: should i give negative reward to agent for suggesting a joint value outside joint limit
counter = 0
limit = []
commedact = []
for jointIndex in self.indexOf_activeJoints:
jointinfo = self.jointInfo.searchBy("jointIndex", jointIndex)[0]
joint_ll = jointinfo["jointLowerLimit"]
joint_ul = jointinfo["jointUpperLimit"]
# print("jointIndex",jointIndex)
# if (jointIndex in cc):
# print("count",jointIndex,"jointName",self.jointInfo.searchBy("jointIndex",jointIndex)[0]["jointName"])
# print("joint_ll",joint_ll)
# print("joint_ul",joint_ul)
limit.append([joint_ll, joint_ul])
if (motorCommands[counter] > joint_ll
or motorCommands[counter] < joint_ll):
commedact.append([motorCommands[counter], True])
else:
commedact.append([motorCommands[counter], False])
# print("---motorCommands:::",motorCommands[counter])
# print("---motorCommands:::",motorCommands[counter])
# print("motorCommands[counter]<=joint_ul ",motorCommands[counter]<=joint_ul )
# print("motorCommands[counter]>=joint_ll",motorCommands[counter]>=joint_ll)
if motorCommands[counter] <= joint_ul and motorCommands[
counter] >= joint_ll:
new_joint_pos[counter] = motorCommands[counter]
counter += 1
#Applying new_joint_pos to hand
counter = 0
# print("\n\n")
# print("action",commedact)
# print("limits",limit)
# print("\n\n")
for jointIndex in self.indexOf_activeJoints:
#print("joint index",self.jointInfo.searchBy("jointIndex",jointIndex)[0]["jointName"],"new_joint_pos::",new_joint_pos[counter]," motorCommands:::",motorCommands[counter])
p.setJointMotorControl2(bodyUniqueId=self.handId,
jointIndex=jointIndex,
controlMode=p.POSITION_CONTROL,
targetPosition=motorCommands[counter],
targetVelocity=0,
force=self.maxForce,
maxVelocity=self.maxVelocity,
positionGain=0.3,
velocityGain=1)
counter += 1
# joint_indexx = self.indexOf_activeJoints[5]
# print("joint index",self.jointInfo.searchBy("jointIndex",joint_indexx)[0])
# p.setJointMotorControl2(bodyUniqueId=self.handId,jointIndex=joint_indexx,
# controlMode=p.POSITION_CONTROL,
# targetPosition=1
# )
"""
def check_collision(Robot, RobotId):
#Getting robot info
jointInfo = JointInfo()
jointInfo.get_infoForAll_joints(Robot)
#getting number of active joints
active_joints_info = jointInfo.getActiveJointsInfo()
num_active_joints = jointInfo.getNumberOfActiveJoints()
parent_list = []
for i in range(num_active_joints):
jointIndex = active_joints_info[i]["jointIndex"]
parent_list.append([
jointIndex,
jointInfo.searchBy("jointIndex", jointIndex)[0]["parentIndex"]
])
collision_set = []
index_of_active_joints = [
active_joints_info[i]["jointIndex"] for i in range(num_active_joints)
]
for i in index_of_active_joints:
for j in index_of_active_joints:
if i == j:
continue
contact = p.getClosestPoints(RobotId, RobotId, -0.001, i, j)
if len(contact) != 0:
collision_set.append([contact[0][3], contact[0][4]])
check_flip = []
for i in range(len(collision_set)):
index_1 = collision_set[i][0]
index_2 = collision_set[i][1]
for j in range(i, len(collision_set)):
if i == j:
continue
if index_1 == collision_set[j][1] and index_2 == collision_set[j][
0]:
check_flip.append(j)
new_check = []
sort = np.argsort(check_flip)
for i in range(len(check_flip)):
new_check.append(check_flip[sort[i]])
for i in range(len(check_flip)):
del collision_set[new_check[i] - i]
check_parent = []
for i in range(len(parent_list)):
index_parent_1 = parent_list[i][0]
index_parent_2 = parent_list[i][1]
for j in range(len(collision_set)):
if index_parent_1 == collision_set[j][
0] and index_parent_2 == collision_set[j][1]:
check_parent.append(j)
if index_parent_1 == collision_set[j][
1] and index_parent_2 == collision_set[j][0]:
check_parent.append(j)
new_check_parent = []
sort_parent = np.argsort(check_parent)
for i in range(len(check_parent)):
new_check_parent.append(check_parent[sort_parent[i]])
for i in range(len(check_parent)):
del collision_set[new_check_parent[i] - i]
collision_result = []
for i in range(len(collision_set)):
index_collision_set_1 = collision_set[i][0]
index_collision_set_2 = collision_set[i][1]
for j in range(num_active_joints):
if index_collision_set_1 == active_joints_info[j]["jointIndex"]:
index_collision_set_1_result = j
if index_collision_set_2 == active_joints_info[j]["jointIndex"]:
index_collision_set_2_result = j
collision_result.append([
active_joints_info[index_collision_set_1_result]["linkName"],
active_joints_info[index_collision_set_2_result]["linkName"]
])
return collision_result
class Kuka_Handlit:
def __init__(self,
ee_xyz,
kuka_loc=None,
urdfRootPath=pybullet_data.getDataPath(),
timeStep=0.01):
self.kuka_loc = kuka_loc
self.urdfRootPath = urdfRootPath
self.timeStep = timeStep
self.maxVelocity = .35
self.maxForce = 200.
self.fingerAForce = 2
self.fingerBForce = 2.5
self.fingerTipForce = 2
self.useSimulation = 1
self.useNullSpace = 21
self.useOrientation = 1
self.kuka_handId = 3
self.kukaEndEffectorIndex = 6
self.kukaGripperIndex = 7
self.ee_xyz = ee_xyz
self.jointInfo = JointInfo()
model_info_path = resource_filename(
__name__, "/kuka_handlit_model/model_info.yml")
self.modelInfo = ModelInfo(model_info_path)
self.fingerTip_link_name = [
"distal_FF", "distal_MF", "distal_RF", "thdistal"
] #this are joints between final link and one before it
#lower limits for null space
self.ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space
self.ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
#joint ranges for null space
self.jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space
self.rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents
self.jd = [
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001
]
self.reset()
def cameraSetup(self):
#https://github.com/bulletphysics/bullet3/issues/1616
width = 128
height = 128
fov = 60 # field of view
aspect = width / height
near = 0.02
far = 1
endEffector_info = p.getLinkState(self.kuka_handId,
self.kukaEndEffectorIndex,
computeForwardKinematics=True)
# print("endEffector_info",endEffector_info)
endEffector_pos = endEffector_info[4]
endEffector_ori = endEffector_info[5]
# print("endEffector_pos",endEffector_pos)
# print("endEffector_ori",endEffector_ori)
endEffector_pos_Xoffset = 0.
endEffector_pos_Zoffset = -0.05
endEffector_pos_InWorldPosition = endEffector_pos
cameraEyePosition = endEffector_pos_InWorldPosition
cameraEyePosition_ = [
endEffector_pos[0] + endEffector_pos_Xoffset, endEffector_pos[1],
endEffector_pos[2] + endEffector_pos_Zoffset
]
rot_matrix = p.getMatrixFromQuaternion(endEffector_ori)
rot_matrix = np.array(rot_matrix).reshape(3, 3)
# Initial vectors
init_camera_vector = (
0, 0, 1
) # modelInfo = ModelInfo(path="./kuka_handlit/model_info.yml")axis
init_up_vector = (0, 1, 0) # y-axis
# Rotated vectors
camera_vector = rot_matrix.dot(init_camera_vector)
up_vector = rot_matrix.dot(init_up_vector)
cameraEyePosition = endEffector_pos_InWorldPosition
dist_cameraTargetPosition = -0.02
view_matrix = p.computeViewMatrix(
cameraEyePosition_, cameraEyePosition + 0.1 * camera_vector,
up_vector)
projection_matrix = p.computeProjectionMatrixFOV(
fov, aspect, near, far)
# Get depth values using the OpenGL renderer
images = p.getCameraImage(width,
height,
view_matrix,
projection_matrix,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_opengl = np.reshape(images[2], (height, width, 4)) * 1. / 255.
depth_buffer_opengl = np.reshape(images[3], [width, height])
depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl)
seg_opengl = np.reshape(images[4], [width, height]) * 1. / 255.
time.sleep(1)
def reset(self):
robot_path = resource_filename(__name__,
"/kuka_handlit_model/model.sdf")
self._kuka_hand = p.loadSDF(robot_path)
self.kuka_handId = self._kuka_hand[0]
if self.kuka_loc != None:
p.resetBasePositionAndOrientation(self.kuka_handId,
self.kuka_loc[0:3],
self.kuka_loc[3:])
self.jointInfo.get_infoForAll_joints(self._kuka_hand)
self.numJoints = p.getNumJoints(self.kuka_handId)
self.num_Active_joint = self.jointInfo.getNumberOfActiveJoints()
self.indexOf_activeJoints = self.jointInfo.getIndexOfActiveJoints()
resetJoints = p.calculateInverseKinematics(self.kuka_handId,
self.kukaEndEffectorIndex,
self.ee_xyz)
self.motorNames = []
self.motorIndices = []
for i in range(self.numJoints):
jointInfo = p.getJointInfo(self.kuka_handId, i)
qIndex = jointInfo[3]
if qIndex > -1:
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
for i in range(self.num_Active_joint):
p.resetJointState(self.kuka_handId, self.indexOf_activeJoints[i],
resetJoints[i])
def getActionDimension(self):
numOf_activeJoints = self.jointInfo.getNumberOfActiveJoints()
return numOf_activeJoints #position x,y,z and roll/pitch/yaw euler angles of end effector
def getObservationDimension(self):
return len(self.getObservation())
def getObservation_joint(self, format="list"):
indexOfActiveJoints = self.jointInfo.getIndexOfActiveJoints()
jointsInfo = self.jointInfo.getActiveJointsInfo()
jointsStates = []
joints_state = {} #key:joint ,value = joint state
for i in range(len(indexOfActiveJoints)):
jointName = jointsInfo[i]["jointName"]
jointIndex = indexOfActiveJoints[i]
jointState = p.getJointState(self._kuka_hand[0], jointIndex)
joints_state[jointName] = jointState[0]
jointsStates.append(jointState[0])
if format == "dictinary":
return joints_state
else:
return jointsStates
def getObservation_palm(self):
Index = self.get_palm_linkIndex()
state = p.getLinkState(self.kuka_handId, Index)
return state[0] + state[1]
def getObservation(self, UseEuler=False):
def finger_obs(UseEuler):
state_dict = {}
observation = []
fingerTipIndexs = self.get_fingerTips_linkIndex()
counter = 0
#getting fingers tip position and orientation
for index in fingerTipIndexs:
state = p.getLinkState(
self.kuka_handId, index
) #mamad:returns endeffector info position orientation
pos = state[0] #mamad: linkWorldPosition
orn = state[1] #mamad: linkWorldOrientation
state_dict[self.fingerTip_link_name[counter]] = {
"pos": pos,
"orn": orn
}
counter += 1
#print("Debug::state_dict",state_dict)
for finger in self.fingerTip_link_name:
euler = p.getEulerFromQuaternion(state_dict[finger]["orn"])
pos = state_dict[finger]["pos"]
observation.extend(list(pos))
if UseEuler == True:
observation.extend(list(euler))
else:
observation.extend(list(state_dict[finger]["orn"]))
return observation
def kuka_obs(UseEuler):
observation = []
kuka_ee_link_Index = self.get_kuka_ee_linkIndex()
state = p.getLinkState(
self.kuka_handId, kuka_ee_link_Index
) #mamad:returns endeffector info position orientation
pos = state[0] #mamad: linkWorldPosition
orn = state[1] #mamad: linkWorldOrientation
euler = p.getEulerFromQuaternion(orn)
observation.extend(list(pos))
if UseEuler == True:
observation.extend(list(euler))
else:
observation.extend(list(orn))
return observation
#self.cameraSetup()
observation = kuka_obs(UseEuler) + finger_obs(UseEuler)
#print("Debug::observation",observation)
#print("Debug::len(observation)",len(observation))
return observation
def applyAction_2(self, motorCommands):
counter = 0
num_active_joints = self.jointInfo.getNumberOfActiveJoints()
active_joints_info = self.jointInfo.getActiveJointsInfo()
for i in range(num_active_joints):
jointIndex = active_joints_info[i]["jointIndex"]
motor_command = motorCommands[counter]
#Applying command for kuka
position = p.calculateInverseKinematics(self.kuka_handId, 7,
[0.5, 0.6, 1.2])
if i <= 6:
p.setJointMotorControl2(self.kuka_handId,
jointIndex,
p.POSITION_CONTROL,
motor_command,
targetVelocity=0,
force=200.0,
maxVelocity=0.35,
positionGain=0.3,
velocityGain=1)
#Appying command for hand
else:
p.setJointMotorControl2(bodyIndex=self.kuka_handId,
jointIndex=jointIndex,
controlMode=p.POSITION_CONTROL,
targetPosition=motor_command)
counter = counter + 1
def applyAction(self, motorCommands):
def apply_A_hand(motorCommands):
"""
the first 16 command will be for the hand. the hand has 16 active joints
The actions are going to Active joint values.
gettting current state of Avtive joints before applying actions This is different that the state we get in getObservation
motorCommands = motorCommands[0:16]
"""
joint_state = [] #current joint postion
new_joint_pos = [0] * len(
self.get_hand_active_joints_index()) # new joint position
for jointIndex in self.get_hand_active_joints_index():
joint_pos = p.getJointState(self.kuka_handId, jointIndex)[0]
joint_state.append(joint_pos)
#making sure the joint values suggested by agent does not exceed joint limit
#design question: should i give negative reward to agent for suggesting a joint value outside joint limit
counter = 0
for jointIndex in self.get_hand_active_joints_index():
jointinfo = self.jointInfo.searchBy("jointIndex",
jointIndex)[0]
joint_ll = jointinfo["jointLowerLimit"]
joint_ul = jointinfo["jointUpperLimit"]
if motorCommands[counter] < joint_ul and motorCommands[
counter] > joint_ll:
new_joint_pos[counter] = joint_state[
counter] + motorCommands[counter]
counter += 1
return new_joint_pos
def apply_A_kuka(motorCommands):
kuka_ee_link_Index = self.get_kuka_ee_linkIndex()
motorCommands = motorCommands[
16:] # the first three command will be xyz and the second three command will be rpy
kuka_ee_index = self.get_kuka_ee_linkIndex()
kuka_ee_state = p.getLinkState(
self.kuka_handId, kuka_ee_link_Index
) #mamad:returns endeffector info position orientation
pos_state = kuka_ee_state[0] #mamad: linkWorldPosition
orn_state = kuka_ee_state[1] #mamad: linkWorldOrientation
pos_command = motorCommands[0:3]
orn_command = motorCommands[3:]
new_pos = pos_command
new_orn = p.getQuaternionFromEuler(orn_command)
#getting joint values using inverse kinematic
jointPoses = p.calculateInverseKinematics(self.kuka_handId,
kuka_ee_index, new_pos,
new_orn)
jointPoses = (-0.36350324105018117, 0.08434877972795868,
1.3253607910308352, -1.3023695529206833,
1.1981002550715345, -2.4402425653234032,
-0.3733762283635729, 0.011011669082923765,
0.01099649407428263, 0.010663992122582346,
0.01090501619723987, -0.405810978009454,
0.7964372388949594, 2.128544355877948e-06,
8.43294195425921e-08, 4.4055107276974525e-07,
0.4617504554232501, 1.3463347345016152e-07,
2.0510552526533784e-06, -1.1743303952431874,
-0.21315269585784075, -0.6981321138974356,
5.54231275569411e-06)
kuka_activeJ = self.get_kuka_Active_joints()
#Applying new_joint_pos to kuka
counter = 0
return list(jointPoses[0:8])
hand_joints_command = apply_A_hand(motorCommands)
kuka_joints_command = apply_A_kuka(motorCommands)
motorCommands = kuka_joints_command + hand_joints_command
counter = 0
num_active_joints = self.jointInfo.getNumberOfActiveJoints()
active_joints_info = self.jointInfo.getActiveJointsInfo()
for i in range(num_active_joints):
jointIndex = active_joints_info[i]["jointIndex"]
motor_command = motorCommands[counter]
p.setJointMotorControl2(self.kuka_handId,
jointIndex,
p.POSITION_CONTROL,
motor_command,
force=1.0)
counter = counter + 1
#utility functions
def get_fingerTips_linkIndex(self):
fingerTips_linkIndex = []
fingerTips_jointInfo = []
fingerTip_joint_name_bytes = [
i.encode(encoding='UTF-8', errors='strict')
for i in self.fingerTip_link_name
]
# getting joints for the final link
for i in fingerTip_joint_name_bytes:
fingerTips_jointInfo.append(
self.jointInfo.searchBy(key="linkName", value=i)[0])
#extracting joint index which equivalent to link index
for i in fingerTips_jointInfo:
fingerTips_linkIndex.append(i["jointIndex"])
return fingerTips_linkIndex
def get_palm_linkIndex(self):
link_name = "palm_fake"
link_name_encoded = link_name.encode(encoding='UTF-8', errors='strict')
palm_link_jointInfo = self.jointInfo.searchBy(
key="linkName", value=link_name_encoded)[0]
palm_link_Index = palm_link_jointInfo["jointIndex"]
return palm_link_Index
def get_kuka_ee_linkIndex(self):
link_name = "lbr_iiwa_link_7"
link_name_encoded = link_name.encode(encoding='UTF-8', errors='strict')
kuka_ee_link_jointInfo = self.jointInfo.searchBy(
key="linkName", value=link_name_encoded)[0]
kuka_ee_link_Index = kuka_ee_link_jointInfo["jointIndex"]
return kuka_ee_link_Index
def get_kuka_Active_joints(self):
link_regex = r'(lbr_iiwa_link_)+[0-7]'
robot_active_joints = self.jointInfo.getActiveJointsInfo()
kuka_active_joints = self.jointInfo.searchBy_regex(
"jointName", link_regex, robot_active_joints)
return kuka_active_joints
def get_hand_active_joints_index(self):
#getting links with active joints
hand_links = self.modelInfo.get_hand_links()
hand_links_info = []
for link_name in hand_links:
link_info = self.modelInfo.searchBy(key="link", value=link_name)
hand_links_info.append(link_info)
hand_links_info_with_active_joints = []
for hand_link_info in hand_links_info:
if hand_link_info["joint"]["j_type"] != "fixed":
hand_links_info_with_active_joints.append(hand_link_info)
hand_indexOf_active_joints = []
for Link in hand_links_info_with_active_joints:
link = self.jointInfo.searchBy("linkName", Link["link_name"])[0]
hand_indexOf_active_joints.append(link["jointIndex"])
return hand_indexOf_active_joints
class Kuka_Handlit:
def __init__(self,
urdfRootPath=pybullet_data.getDataPath(),
timeStep=0.01):
self.urdfRootPath = urdfRootPath
self.timeStep = timeStep
self.maxVelocity = .35
self.maxForce = 200.
self.fingerAForce = 2
self.fingerBForce = 2.5
self.fingerTipForce = 2
self.useSimulation = 1
self.useNullSpace = 21
self.useOrientation = 1
self.kuka_handId = 3
self.kukaEndEffectorIndex = 6
self.kukaGripperIndex = 7
self.jointInfo = JointInfo()
self.modelInfo = ModelInfo(path="./model_info.yml")
self.fingerTip_link_name = [
"lh_ffdistal", "lh_mfdistal", "lh_rfdistal", "lh_thdistal"
] #this are joints between final link and one before it
#lower limits for null space
self.ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space
self.ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
#joint ranges for null space
self.jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space
self.rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents
self.jd = [
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001
]
self.reset()
def cameraSetup(self):
#https://github.com/bulletphysics/bullet3/issues/1616
width = 128
height = 128
fov = 60 # field of view
aspect = width / height
near = 0.02
far = 1
endEffector_info = p.getLinkState(self.kuka_handId,
self.kukaEndEffectorIndex,
computeForwardKinematics=True)
# print("endEffector_info",endEffector_info)
endEffector_pos = endEffector_info[4]
endEffector_ori = endEffector_info[5]
# print("endEffector_pos",endEffector_pos)
# print("endEffector_ori",endEffector_ori)
endEffector_pos_Xoffset = 0.
endEffector_pos_Zoffset = -0.05
endEffector_pos_InWorldPosition = endEffector_pos
cameraEyePosition = endEffector_pos_InWorldPosition
cameraEyePosition_ = [
endEffector_pos[0] + endEffector_pos_Xoffset, endEffector_pos[1],
endEffector_pos[2] + endEffector_pos_Zoffset
]
rot_matrix = p.getMatrixFromQuaternion(endEffector_ori)
rot_matrix = np.array(rot_matrix).reshape(3, 3)
# Initial vectors
init_camera_vector = (
0, 0, 1
) # modelInfo = ModelInfo(path="./kuka_handlit/model_info.yml")axis
init_up_vector = (0, 1, 0) # y-axis
# Rotated vectors
camera_vector = rot_matrix.dot(init_camera_vector)
up_vector = rot_matrix.dot(init_up_vector)
cameraEyePosition = endEffector_pos_InWorldPosition
dist_cameraTargetPosition = -0.02
view_matrix = p.computeViewMatrix(
cameraEyePosition_, cameraEyePosition + 0.1 * camera_vector,
up_vector)
projection_matrix = p.computeProjectionMatrixFOV(
fov, aspect, near, far)
# Get depth values using the OpenGL renderer
images = p.getCameraImage(width,
height,
view_matrix,
projection_matrix,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_opengl = np.reshape(images[2], (height, width, 4)) * 1. / 255.
depth_buffer_opengl = np.reshape(images[3], [width, height])
depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl)
seg_opengl = np.reshape(images[4], [width, height]) * 1. / 255.
time.sleep(1)
def reset(self):
self._kuka_hand = p.loadSDF("model.sdf")
self.kuka_handId = self._kuka_hand[0]
print("self._kuka_hand", self._kuka_hand)
print("self.kuka_handId", self.kuka_handId)
#self.cameraSetup()
# reset joints to the middle valu
p.resetBasePositionAndOrientation(
self.kuka_handId, [-0.100000, 0.000000, 0.070000],
[0.000000, 0.000000, 0.000000, 1.000000])
self.jointInfo.get_infoForAll_joints(self._kuka_hand)
self.numJoints = p.getNumJoints(self.kuka_handId)
self.num_Active_joint = self.jointInfo.getNumberOfActiveJoints()
self.indexOf_activeJoints = self.jointInfo.getIndexOfActiveJoints()
self.motorNames = []
self.motorIndices = []
for i in range(self.numJoints):
jointInfo = p.getJointInfo(self.kuka_handId, i)
qIndex = jointInfo[3]
if qIndex > -1:
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
def getActionDimension(self):
numOf_activeJoints = self.jointInfo.getNumberOfActiveJoints()
return numOf_activeJoints #position x,y,z and roll/pitch/yaw euler angles of end effector
def getObservationDimension(self):
return len(self.getObservation())
def getObservation(self):
def finger_obs():
state_dict = {}
observation = []
fingerTipIndexs = self.get_fingerTips_linkIndex()
#print("Debug::fingerTipIndexs",fingerTipIndexs)
counter = 0
#getting fingers tip position and orientation
for index in fingerTipIndexs:
state = p.getLinkState(
self.kuka_handId, index
) #mamad:returns endeffector info position orientation
pos = state[0] #mamad: linkWorldPosition
orn = state[1] #mamad: linkWorldOrientation
state_dict[self.fingerTip_link_name[counter]] = {
"pos": pos,
"orn": orn
}
counter += 1
#print("Debug::state_dict",state_dict)
for finger in self.fingerTip_link_name:
euler = p.getEulerFromQuaternion(state_dict[finger]["orn"])
pos = state_dict[finger]["pos"]
observation.extend(list(pos))
observation.extend(list(euler))
return observation
def kuka_obs():
observation = []
kuka_ee_link_Index = self.get_kuka_ee_linkIndex()
state = p.getLinkState(
self.kuka_handId, kuka_ee_link_Index
) #mamad:returns endeffector info position orientation
pos = state[0] #mamad: linkWorldPosition
orn = state[1] #mamad: linkWorldOrientation
euler = p.getEulerFromQuaternion(orn)
observation.extend(list(pos))
observation.extend(list(euler))
return observation
#self.cameraSetup()
observation = kuka_obs() + finger_obs()
#print("Debug::observation",observation)
#print("Debug::len(observation)",len(observation))
return observation
def applyAction(self, motorCommands):
#print("controller Motorcommand",motorCommands)
#print("controller Motorcommand len",len(motorCommands))
def apply_A_hand(motorCommands):
# the first 16 command will be for the hand. the hand has 16 active joints
#The actions are going to Active joint values.
#gettting current state of Avtive joints before applying actions This is different that the state we get in getObservation
motorCommands = motorCommands[0:16]
print("hand_motor_commands", len(motorCommands))
joint_state = [] #current joint postion
new_joint_pos = [0] * len(
self.get_hand_active_joints_index()) # new joint position
for jointIndex in self.get_hand_active_joints_index():
joint_pos = p.getJointState(self.kuka_handId, jointIndex)[0]
joint_state.append(joint_pos)
#print("Debug::joint_state",joint_state)
#making sure the joint values suggested by agent does not exceed joint limit
#design question: should i give negative reward to agent for suggesting a joint value outside joint limit
counter = 0
print("get_hand_active_joints_index()",
self.get_hand_active_joints_index)
for jointIndex in self.get_hand_active_joints_index():
jointinfo = self.jointInfo.searchBy("jointIndex",
jointIndex)[0]
joint_ll = jointinfo["jointLowerLimit"]
joint_ul = jointinfo["jointUpperLimit"]
if motorCommands[counter] < joint_ul and motorCommands[
counter] > joint_ll:
new_joint_pos[counter] = joint_state[
counter] + motorCommands[counter]
counter += 1
return new_joint_pos
def apply_A_kuka(motorCommands):
kuka_ee_link_Index = self.get_kuka_ee_linkIndex()
motorCommands = motorCommands[
16:] # the first three command will be xyz and the second three command will be rpy
kuka_ee_index = self.get_kuka_ee_linkIndex()
kuka_ee_state = p.getLinkState(
self.kuka_handId, kuka_ee_link_Index
) #mamad:returns endeffector info position orientation
pos_state = kuka_ee_state[0] #mamad: linkWorldPosition
orn_state = kuka_ee_state[1] #mamad: linkWorldOrientation
pos_command = motorCommands[0:3]
orn_command = motorCommands[3:]
new_pos = pos_command
new_orn = new_pos
#getting joint values using inverse kinematic
jointPoses = p.calculateInverseKinematics(self.kuka_handId,
kuka_ee_index, new_pos,
new_orn)
kuka_activeJ = self.get_kuka_Active_joints()
#Applying new_joint_pos to kuka
counter = 0
return jointPoses
hand_joints_command = apply_A_hand(motorCommands)
kuka_joints_command = apply_A_kuka(motorCommands)
motorCommands = hand_joints_command + kuka_joints_command
counter = 0
num_active_joints = jointInfo.getNumberOfActiveJoints()
for i in range(num_active_joints):
jointIndex = active_joints_info[i]["jointIndex"]
motor_command = motorCommands[counter]
# print("motor_command",motor_command)
p.setJointMotorControl2(self.kuka_handId,
jointIndex,
p.POSITION_CONTROL,
motor_command,
force=1.0)
counter = counter + 1
#utility functions
def get_fingerTips_linkIndex(self):
fingerTips_linkIndex = []
fingerTips_linkInfo = []
fingerTip_link_name_bytes = [
i.encode(encoding='UTF-8', errors='strict')
for i in self.fingerTip_link_name
]
# getting joints for the final link
for i in fingerTip_link_name_bytes:
fingerTips_linkInfo.append(
self.jointInfo.searchBy(key="linkName", value=i))
#print("Debug8:: ",fingerTips_linkInfo)
#extracting joint index which equivalent to link index
#print("Debug13::len(fingerTips_linkInfo)",len(fingerTips_linkInfo))
for i in fingerTips_linkInfo:
i = i[0]
#print("Debug11 i",i)
#print("Debug12::i['jointIndex']",i["jointIndex"])
fingerTips_linkIndex.append(i["jointIndex"])
#print("Debug14::fingerTips_linkIndex",fingerTips_linkIndex)
return fingerTips_linkIndex
def get_kuka_ee_linkIndex(self):
link_name = "lbr_iiwa_link_7"
link_name_encoded = link_name.encode(encoding='UTF-8', errors='strict')
kuka_ee_link_jointInfo = self.jointInfo.searchBy(
key="linkName", value=link_name_encoded)[0]
#kuka_ee_liml_jointLLimit = self.jointInfo.getJointLimits(key="linkName",value =link_name_encoded )
print("Debug::kuka_ee_link_jointInfo", self.jointInfo)
#print("Debug::kuka_ee_liml_jointLLimit",kuka_ee_liml_jointLLimit)
#print("Debug2::AllJoint",self.jointInfo.get_stored_joints())
#print("Debug1::kuka_ee_link_jointInfo",kuka_ee_link_jointInfo)
kuka_ee_link_Index = kuka_ee_link_jointInfo["jointIndex"]
#print("Debug2::kuka_ee_link_Index",kuka_ee_link_Index)
return kuka_ee_link_Index
def get_kuka_Active_joints(self):
link_regex = r'(lbr_iiwa_link_)+[0-7]'
robot_active_joints = self.jointInfo.getActiveJointsInfo()
kuka_active_joints = self.jointInfo.searchBy_regex(
"jointIndex", link_regex, robot_active_joints)
return kuka_active_joints
def get_hand_active_joints_index(self):
#getting links with active joints
hand_links = self.modelInfo.get_hand_links()
hand_links_info = []
for link_name in hand_links:
link_info = self.modelInfo.searchBy(key="link", value=link_name)
hand_links_info.append(link_info)
hand_links_info_with_active_joints = []
for hand_link_info in hand_links_info:
if hand_link_info["joint"]["j_type"] != "fixed":
hand_links_info_with_active_joints.append(hand_link_info)
hand_indexOf_active_joints = []
for Link in hand_links_info_with_active_joints:
link = self.jointInfo.searchBy("linkName", Link["link_name"])[0]
hand_indexOf_active_joints.append(link["jointIndex"])
print("hand_indexOf_active_joints", hand_indexOf_active_joints)
return hand_indexOf_active_joints