def joinUrdf(self, childEditor, parentLinkIndex=0, jointPivotXYZInParent=[0,0,0], jointPivotRPYInParent=[0,0,0], jointPivotXYZInChild=[0,0,0], jointPivotRPYInChild=[0,0,0], parentPhysicsClientId=0, childPhysicsClientId=0): childLinkIndex = len(self.urdfLinks) insertJointIndex = len(self.urdfJoints) #combine all links, and add a joint for link in childEditor.urdfLinks: self.linkNameToIndex[link.link_name]=len(self.urdfLinks) self.urdfLinks.append(link) for joint in childEditor.urdfJoints: self.urdfJoints.append(joint) #add a new joint between a particular jointPivotQuatInChild = p.getQuaternionFromEuler(jointPivotRPYInChild) invJointPivotXYZInChild, invJointPivotQuatInChild = p.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild) #apply this invJointPivot***InChild to all inertial, visual and collision element in the child link #inertial pos, orn = p.multiplyTransforms(self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz,p.getQuaternionFromEuler(self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild, physicsClientId=parentPhysicsClientId) self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz = pos self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy = p.getEulerFromQuaternion(orn) #all visual for v in self.urdfLinks[childLinkIndex].urdf_visual_shapes: pos, orn = p.multiplyTransforms(v.origin_xyz,p.getQuaternionFromEuler(v.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild) v.origin_xyz = pos v.origin_rpy = p.getEulerFromQuaternion(orn) #all collision for c in self.urdfLinks[childLinkIndex].urdf_collision_shapes: pos, orn = p.multiplyTransforms(c.origin_xyz,p.getQuaternionFromEuler(c.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild) c.origin_xyz = pos c.origin_rpy = p.getEulerFromQuaternion(orn) childLink = self.urdfLinks[childLinkIndex] parentLink = self.urdfLinks[parentLinkIndex] joint = UrdfJoint() joint.link = childLink joint.joint_name = "joint_dummy1" joint.joint_type = p.JOINT_REVOLUTE joint.joint_lower_limit = 0 joint.joint_upper_limit = -1 joint.parent_name = parentLink.link_name joint.child_name = childLink.link_name joint.joint_origin_xyz = jointPivotXYZInParent joint.joint_origin_rpy = jointPivotRPYInParent joint.joint_axis_xyz = [0,0,1] #the following commented line would crash PyBullet, it messes up the joint indexing/ordering #self.urdfJoints.append(joint) #so make sure to insert the joint in the right place, to links/joints match self.urdfJoints.insert(insertJointIndex,joint) return joint
def initializeFromBulletBody(self, bodyUid, physicsClientId):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
self.linkNameToIndex[baseLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(baseLink)
#print(visualShapes)
#optionally create child links and joints
for j in range(p.getNumJoints(bodyUid,physicsClientId=physicsClientId)):
jointInfo = p.getJointInfo(bodyUid,j,physicsClientId=physicsClientId)
urdfLink = UrdfLink()
self.convertLinkFromMultiBody(bodyUid, j, urdfLink,physicsClientId)
urdfLink.link_name = jointInfo[12].decode("utf-8")
self.linkNameToIndex[urdfLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(urdfLink)
urdfJoint = UrdfJoint()
urdfJoint.link = urdfLink
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
urdfJoint.joint_type = jointInfo[2]
urdfJoint.joint_axis_xyz = jointInfo[13]
orgParentIndex = jointInfo[16]
if (orgParentIndex<0):
urdfJoint.parent_name = baseLink.link_name
else:
parentJointInfo = p.getJointInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dynChild = p.getDynamicsInfo(bodyUid,j,physicsClientId=physicsClientId)
childInertiaPos = dynChild[3]
childInertiaOrn = dynChild[4]
parentCom2JointPos=jointInfo[14]
parentCom2JointOrn=jointInfo[15]
tmpPos,tmpOrn = p.multiplyTransforms(childInertiaPos,childInertiaOrn,parentCom2JointPos,parentCom2JointOrn)
tmpPosInv,tmpOrnInv = p.invertTransform(tmpPos,tmpOrn)
dynParent = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
parentInertiaPos = dynParent[3]
parentInertiaOrn = dynParent[4]
pos,orn = p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, tmpPosInv, tmpOrnInv)
pos,orn_unused=p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, parentCom2JointPos,[0,0,0,1])
urdfJoint.joint_origin_xyz = pos
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
self.urdfJoints.append(urdfJoint)
def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
gripperState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaGripperIndex)
gripperPos = gripperState[0]
gripperOrn = gripperState[1]
blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
invGripperPos,invGripperOrn = p.invertTransform(gripperPos,gripperOrn)
gripperMat = p.getMatrixFromQuaternion(gripperOrn)
dir0 = [gripperMat[0],gripperMat[3],gripperMat[6]]
dir1 = [gripperMat[1],gripperMat[4],gripperMat[7]]
dir2 = [gripperMat[2],gripperMat[5],gripperMat[8]]
gripperEul = p.getEulerFromQuaternion(gripperOrn)
#print("gripperEul")
#print(gripperEul)
blockPosInGripper,blockOrnInGripper = p.multiplyTransforms(invGripperPos,invGripperOrn,blockPos,blockOrn)
projectedBlockPos2D =[blockPosInGripper[0],blockPosInGripper[1]]
blockEulerInGripper = p.getEulerFromQuaternion(blockOrnInGripper)
#print("projectedBlockPos2D")
#print(projectedBlockPos2D)
#print("blockEulerInGripper")
#print(blockEulerInGripper)
#we return the relative x,y position and euler angle of block in gripper space
blockInGripperPosXYEulZ =[blockPosInGripper[0],blockPosInGripper[1],blockEulerInGripper[2]]
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1)
self._observation.extend(list(blockInGripperPosXYEulZ))
return self._observation
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink, physicsClientId):
dyn = p.getDynamicsInfo(bodyUid,linkIndex,physicsClientId=physicsClientId)
urdfLink.urdf_inertial.mass = dyn[0]
urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
#todo
urdfLink.urdf_inertial.origin_xyz = dyn[3]
rpy = p.getEulerFromQuaternion(dyn[4])
urdfLink.urdf_inertial.origin_rpy = rpy
visualShapes = p.getVisualShapeData(bodyUid,physicsClientId=physicsClientId)
matIndex = 0
for v in visualShapes:
if (v[1]==linkIndex):
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
if (v[2]==p.GEOM_BOX):
urdfVisual.geom_extents = v[3]
if (v[2]==p.GEOM_SPHERE):
urdfVisual.geom_radius = v[3][0]
if (v[2]==p.GEOM_MESH):
urdfVisual.geom_meshfilename = v[4].decode("utf-8")
urdfVisual.geom_meshscale = v[3]
if (v[2]==p.GEOM_CYLINDER):
urdfVisual.geom_length=v[3][0]
urdfVisual.geom_radius=v[3][1]
if (v[2]==p.GEOM_CAPSULE):
urdfVisual.geom_length=v[3][0]
urdfVisual.geom_radius=v[3][1]
urdfVisual.origin_xyz = v[5]
urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
urdfVisual.material_rgba = v[7]
name = 'mat_{}_{}'.format(linkIndex,matIndex)
urdfVisual.material_name = name
urdfLink.urdf_visual_shapes.append(urdfVisual)
matIndex=matIndex+1
collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex,physicsClientId=physicsClientId)
for v in collisionShapes:
urdfCollision = UrdfCollision()
urdfCollision.geom_type = v[2]
if (v[2]==p.GEOM_BOX):
urdfCollision.geom_extents = v[3]
if (v[2]==p.GEOM_SPHERE):
urdfCollision.geom_radius = v[3][0]
if (v[2]==p.GEOM_MESH):
urdfCollision.geom_meshfilename = v[4].decode("utf-8")
urdfCollision.geom_meshscale = v[3]
if (v[2]==p.GEOM_CYLINDER):
urdfCollision.geom_length=v[3][0]
urdfCollision.geom_radius=v[3][1]
if (v[2]==p.GEOM_CAPSULE):
urdfCollision.geom_length=v[3][0]
urdfCollision.geom_radius=v[3][1]
pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
v[5], v[6])
urdfCollision.origin_xyz = pos
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
urdfLink.urdf_collision_shapes.append(urdfCollision)
def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
eeState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
endEffectorPos = eeState[0]
endEffectorOrn = eeState[1]
blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
invEEPos,invEEOrn = p.invertTransform(endEffectorPos,endEffectorOrn)
blockPosInEE,blockOrnInEE = p.multiplyTransforms(invEEPos,invEEOrn,blockPos,blockOrn)
blockEulerInEE = p.getEulerFromQuaternion(blockOrnInEE)
self._observation.extend(list(blockPosInEE))
self._observation.extend(list(blockEulerInEE))
return self._observation
def stepSim(self):
self.robot.updateSpringForce()
p.stepSimulation(physicsClientId=self.physicsClientId)
self.lastStateRecordFlag = False
if self.camFollowBot:
pose = self.robot.getPositionOrientation()
pos = pose[0]
orien = pose[1]
forwardDir = p.multiplyTransforms([0, 0, 0], orien, [1, 0, 0],
[0, 0, 0, 1])[0]
headingAngle = np.arctan2(forwardDir[1],
forwardDir[0]) * 180 / np.pi - 90
p.resetDebugVisualizerCamera(1.0,
headingAngle,
-15,
pos,
physicsClientId=self.physicsClientId)
def generate_target(self):
# Set target on mouth
self.mouth_pos = [0, -0.11, 0.03] if self.human.gender == 'male' else [
0, -0.1, 0.03
]
head_pos, head_orient = self.human.get_pos_orient(self.human.head)
target_pos, target_orient = p.multiplyTransforms(
head_pos,
head_orient,
self.mouth_pos, [0, 0, 0, 1],
physicsClientId=self.id)
self.target = self.create_sphere(radius=0.01,
mass=0.0,
pos=target_pos,
collision=False,
rgba=[0, 1, 0, 1])
self.update_targets()
def occluding(object,
camera_position_and_orientation,
front_facing_axis,
world=None):
"""
This reasoning query lists the objects which are occluding a given object. This works similar to 'visible'.
First the object alone will be rendered and the position of the pixels of the object in the picture will be saved.
After that the complete scene will be rendered and the previous saved pixel positions will be compared to the
actual pixels, if in one pixel an other object is visible ot will be saved as occluding.
:param object: The object for which occluding should be checked
:param camera_position_and_orientation: The position and orientation of the
camera in world coordinate frame
:front_facing_axis: The axis, of the camera frame, which faces to the front of
the robot. Given as list of x, y, z
:param world: The BulletWorld if more than one BulletWorld is active
:return: A list of occluding objects
"""
world, world_id = _world_and_id(world)
state = p.saveState(physicsClientId=world_id)
for obj in world.objects:
if obj.id is not object.id:
# p.removeBody(object.id, physicsClientId=world_id)
# Hot fix until I come up with something better
p.resetBasePositionAndOrientation(obj.id, [100, 100, 100],
[0, 0, 0, 1], world_id)
world_T_cam = camera_position_and_orientation
cam_T_point = list(np.multiply(front_facing_axis, 2))
target_point = p.multiplyTransforms(world_T_cam[0], world_T_cam[1],
cam_T_point, [0, 0, 0, 1])
seg_mask = _get_seg_mask_for_target(target_point, world_T_cam, world)
pixels = []
for i in range(0, 256):
for j in range(0, 256):
if seg_mask[i][j] == object.id:
pixels.append((i, j))
p.restoreState(state, physicsClientId=world_id)
occluding = []
seg_mask = _get_seg_mask_for_target(target_point, world_T_cam, world)
for c in pixels:
if not seg_mask[c[0]][c[1]] == object.id:
occluding.append(seg_mask[c[0]][c[1]])
return list(set(map(lambda x: world.get_object_by_id(x), occluding)))
def get_world_visual_pose(body_unique_id: int, link_index: int):
"""Pose of visual frame with respect to world frame expressed in world frame.
Args:
body_unique_id (int): The body unique id, as returned by loadURDF, etc.
link_index (int): Link index or -1 for the base.
Returns:
pos_orn (tuple): See description.
"""
world_link_pose = get_world_link_pose(body_unique_id, link_index)
visual_shape_data = p.getVisualShapeData(body_unique_id, link_index)
local_visual_pose = (visual_shape_data[link_index + 1][5],
visual_shape_data[link_index + 1][6])
return p.multiplyTransforms(world_link_pose[0], world_link_pose[1],
local_visual_pose[0], local_visual_pose[1])
def sensedHeightMap(self,sensorPose,mapDim,mapResolution):
maxRadius = np.sqrt((mapDim[0]/2.)**2+(mapDim[1]/2.)**2)
vecX = self.gridX.reshape(-1)-sensorPose[0][0]
vecY = self.gridY.reshape(-1)-sensorPose[0][1]
indices = np.all(np.stack((np.abs(vecX)<=(maxRadius+self.meshScale[0]),np.abs(vecY)<=(maxRadius+self.meshScale[1]))),axis=0)
vecX = vecX[indices]
vecY = vecY[indices]
vecZ = self.gridZ.reshape(-1)[indices]
forwardDir = np.array(p.multiplyTransforms([0,0,0],sensorPose[1],[1,0,0],[0,0,0,1])[0][0:2])
headingAngle = np.arctan2(forwardDir[1],forwardDir[0])
relativeX = vecX*np.cos(headingAngle)+vecY*np.sin(headingAngle)
relativeY = -vecX*np.sin(headingAngle)+vecY*np.cos(headingAngle)
rMapX = np.linspace(-mapDim[0]/2.,mapDim[0]/2.,mapResolution[0])
rMapY = np.linspace(-mapDim[1]/2.,mapDim[1]/2.,mapResolution[1])
points = np.stack((relativeX,relativeY)).transpose()
rMapX,rMapY = np.meshgrid(rMapX,rMapY)
return griddata(points, vecZ, (rMapX,rMapY))-sensorPose[0][2]
def _set_attached_objects(self, prev_object):
"""
This method updates the positions of all attached objects. This is done
by calculating the new pose in world coordinate frame and setting the
base pose of the attached objects to this new pose.
After this the _set_attached_objects method of all attached objects
will be called.
:param prev_object: The object that called this method, this will be
excluded to prevent recursion in the update.
"""
for obj in self.attachments:
if obj in prev_object:
continue
if self.attachments[obj][2]:
# Updates the attachment transformation and contraint if the
# attachment is loos, instead of updating the position of all attached objects
link_T_object = self._calculate_transform(
obj, self.attachments[obj][1])
link_id = self.get_link_id(
self.attachments[obj]
[1]) if self.attachments[obj][1] else -1
self.attachments[obj][0] = link_T_object
obj.attachments[self][0] = p.invertTransform(
link_T_object[0], link_T_object[1])
p.removeConstraint(self.cids[obj])
cid = p.createConstraint(self.id, link_id, obj.id, -1,
p.JOINT_FIXED, [0, 0, 0],
link_T_object[0], [0, 0, 0],
link_T_object[1])
self.cids[obj] = cid
obj.cids[self] = cid
else:
# Updates the position of all attached objects
link_T_object = self.attachments[obj][0]
link_name = self.attachments[obj][1]
world_T_link = self.get_link_position_and_orientation(
link_name
) if link_name else self.get_position_and_orientation()
world_T_object = p.multiplyTransforms(world_T_link[0],
world_T_link[1],
link_T_object[0],
link_T_object[1])
p.resetBasePositionAndOrientation(obj.id, world_T_object[0],
world_T_object[1])
obj._set_attached_objects(prev_object + [self])
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink,
physicsClientId):
dyn = p.getDynamicsInfo(bodyUid,
linkIndex,
physicsClientId=physicsClientId)
urdfLink.urdf_inertial.mass = dyn[0]
urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
urdfLink.urdf_inertial.origin_xyz = dyn[3]
rpy = p.getEulerFromQuaternion(dyn[4])
urdfLink.urdf_inertial.origin_rpy = rpy
visualShapes = p.getVisualShapeData(bodyUid,
physicsClientId=physicsClientId)
matIndex = 0
for i in range(len(visualShapes)):
v = visualShapes[i]
if (v[1] == linkIndex and v[2] == p.GEOM_MESH):
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
urdfVisual.geom_meshfilename = v[4].decode("utf-8")
urdfVisual.geom_meshscale = v[3]
urdfVisual.origin_xyz = v[5]
urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
urdfVisual.material_rgba = v[7]
name = 'mat_{}_{}'.format(linkIndex, matIndex)
urdfVisual.material_name = name
urdfLink.urdf_visual_shapes.append(urdfVisual)
matIndex = matIndex + 1
collisionShapes = p.getCollisionShapeData(
bodyUid, linkIndex, physicsClientId=physicsClientId)
for i in range(len(collisionShapes)):
v = collisionShapes[i]
if (v[2] == p.GEOM_MESH):
urdfCollision = UrdfCollision()
urdfCollision.geom_type = v[2]
urdfCollision.geom_meshfilename = urdfLink.urdf_visual_shapes[
i].geom_meshfilename
urdfCollision.geom_meshscale = v[3]
pos, orn = p.multiplyTransforms(dyn[3], dyn[4], \
v[5], v[6])
urdfCollision.origin_xyz = pos
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
urdfLink.urdf_collision_shapes.append(urdfCollision)
def get_transform(self):
if self.robot is not None:
self.pos_offset = self.robot.tool_pos_offset[self.task]
self.orient_offset = self.get_quaternion(
self.robot.tool_orient_offset[self.task])
gripper_pos, gripper_orient = self.robot.get_pos_orient(
self.robot.right_tool_joint
if self.right else self.robot.left_tool_joint,
center_of_mass=True)
transform_pos, transform_orient = p.multiplyTransforms(
positionA=gripper_pos,
orientationA=gripper_orient,
positionB=self.pos_offset,
orientationB=self.orient_offset,
physicsClientId=self.id)
else:
transform_pos = [0, 0, 0]
transform_orient = [0, 0, 0, 1]
return transform_pos, transform_orient
def initializeFromBulletBody(self, bodyUid):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink)
baseLink.link_name = p.getBodyInfo(bodyUid)[0].decode("utf-8")
self.urdfLinks.append(baseLink)
#print(visualShapes)
#optionally create child links and joints
for j in range(p.getNumJoints(bodyUid)):
jointInfo = p.getJointInfo(bodyUid, j)
urdfLink = UrdfLink()
self.convertLinkFromMultiBody(bodyUid, j, urdfLink)
urdfLink.link_name = jointInfo[12].decode("utf-8")
self.urdfLinks.append(urdfLink)
urdfJoint = UrdfJoint()
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
urdfJoint.joint_type = jointInfo[2]
urdfJoint.joint_axis_xyz = jointInfo[13]
parentIndex = jointInfo[16]
if (parentIndex < 0):
urdfJoint.parent_name = baseLink.link_name
else:
parentJointInfo = p.getJointInfo(bodyUid, parentIndex)
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dyn = p.getDynamicsInfo(bodyUid, parentIndex)
parentInertiaPos = dyn[3]
parentInertiaOrn = dyn[4]
pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
jointInfo[14], jointInfo[15])
urdfJoint.joint_origin_xyz = pos
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
self.urdfJoints.append(urdfJoint)
def get_world_collision_pose(body_unique_id: int, link_index: int):
"""Pose of collision frame with respect to world frame expressed in world frame.
Args:
body_unique_id (int): The body unique id, as returned by loadURDF, etc.
link_index (int): Link index or -1 for the base.
Returns:
pos_orn (tuple of ): See description.
"""
world_inertial_pose = get_world_inertial_pose(body_unique_id, link_index)
collision_shape_data = p.getCollisionShapeData(body_unique_id, link_index)
if len(collision_shape_data) > 1:
raise NotImplementedError
local_collision_pose = (collision_shape_data[0][5],
collision_shape_data[0][6])
return p.multiplyTransforms(world_inertial_pose[0], world_inertial_pose[1],
local_collision_pose[0],
local_collision_pose[1])
def _get_obs(self):
image_raw = p.getCameraImage(self.image_dim, self.image_dim,
self._CameraViewMatrix,
self._CameraProjMatrix)
image_obs = image_raw[2].flatten()
cupPos, cupOrn = p.getBasePositionAndOrientation(self.teacupUid)
kinovaState = self.kinova.GetObservation()
# relative position
EndEffector_pos, EndEffector_orn = self.kinova.EndEffectorObersavations(
)
invEndEffector_pos, invEndEffector_orn = p.invertTransform(
EndEffector_pos, EndEffector_orn)
cupPosInEE, cupOrnInEE = p.multiplyTransforms(invEndEffector_pos,
invEndEffector_orn,
cupPos, cupOrn)
self._observation = \
np.concatenate([np.array(cupPos), np.array(cupOrn), np.array(cupPosInEE), np.array(cupOrnInEE), np.array(kinovaState), image_obs]).flatten()
return self._observation
def __init__(self, path, scale=1., position=[0, 0, 0], orientation=[0, 0, 0]):
self.filename = path
self.scale = scale
self.position = position
self.orientation = orientation
self._default_mass = 3.
self._default_transform = {
'position': [0, 0, 0],
'orientation_quat': [1. / np.sqrt(2), 0, 0, 1. / np.sqrt(2)],
}
pose = p.multiplyTransforms(positionA=self.position,
orientationA=p.getQuaternionFromEuler(self.orientation),
positionB=self._default_transform['position'],
orientationB=self._default_transform['orientation_quat'])
self.pose = {
'position': pose[0],
'orientation_quat': pose[1],
}
def getExtendedObservation(self):
""" get block position in gripper frame - assumes knowledge of block position """
self._observation = self._kuka.getObservation()
# observe EEF
gripperState = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaGripperIndex)
gripperPos = gripperState[0]
gripperOrn = gripperState[1]
# observe block
blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid)
invGripperPos, invGripperOrn = p.invertTransform(gripperPos, gripperOrn)
gripperMat = p.getMatrixFromQuaternion(gripperOrn)
dir0 = [gripperMat[0], gripperMat[3], gripperMat[6]]
dir1 = [gripperMat[1], gripperMat[4], gripperMat[7]]
dir2 = [gripperMat[2], gripperMat[5], gripperMat[8]]
gripperEul = p.getEulerFromQuaternion(gripperOrn)
#print("gripperEul")
#print(gripperEul)
# convert block coordinates to gripper frame
blockPosInGripper, blockOrnInGripper = p.multiplyTransforms(invGripperPos, invGripperOrn,
blockPos, blockOrn)
projectedBlockPos2D = [blockPosInGripper[0], blockPosInGripper[1]]
blockEulerInGripper = p.getEulerFromQuaternion(blockOrnInGripper)
#print("projectedBlockPos2D")
#print(projectedBlockPos2D)
#print("blockEulerInGripper")
#print(blockEulerInGripper)
#we return the relative x,y position and euler angle of block in gripper space
blockInGripperPosXYEulZ = [blockPosInGripper[0], blockPosInGripper[1], blockEulerInGripper[2]]
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1)
self._observation.extend(list(blockInGripperPosXYEulZ))
return self._observation
def init_tool(self, robot, mesh_scale=[1]*3, pos_offset=[0]*3, orient_offset=[0, 0, 0, 1], left=True, maximal=False, alpha=1.0):
if left:
gripper_pos, gripper_orient = p.getLinkState(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2]
else:
gripper_pos, gripper_orient = p.getLinkState(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2]
transform_pos, transform_orient = p.multiplyTransforms(positionA=gripper_pos, orientationA=gripper_orient, positionB=pos_offset, orientationB=orient_offset, physicsClientId=self.id)
if self.task == 'scratch_itch':
tool = p.loadURDF(os.path.join(self.directory, 'scratcher', 'tool_scratch.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
elif self.task == 'bed_bathing':
tool = p.loadURDF(os.path.join(self.directory, 'bed_bathing', 'wiper.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
elif self.task == 'bite_transfer' and self.robot_type == "panda":
tool = p.loadURDF(os.path.join(self.directory, 'dinnerware', 'drop_fork.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
elif self.task in ['drinking', 'scooping', 'feeding', 'arm_manipulation']:
if self.task == 'drinking':
visual_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup.obj')
collision_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup_vhacd.obj')
elif self.task in ['scooping', 'feeding']:
visual_filename = os.path.join(self.directory, 'dinnerware', 'spoon_reduced_compressed.obj')
collision_filename = os.path.join(self.directory, 'dinnerware', 'spoon_vhacd.obj')
elif self.task == 'arm_manipulation':
visual_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper.obj')
collision_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper_vhacd.obj')
tool_visual = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=visual_filename, meshScale=mesh_scale, rgbaColor=[1, 1, 1, alpha], physicsClientId=self.id)
tool_collision = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=collision_filename, meshScale=mesh_scale, physicsClientId=self.id)
tool = p.createMultiBody(baseMass=0.01, baseCollisionShapeIndex=tool_collision, baseVisualShapeIndex=tool_visual, basePosition=transform_pos, baseOrientation=transform_orient, useMaximalCoordinates=maximal, physicsClientId=self.id)
if left:
# Disable collisions between the tool and robot
for j in (range(71, 86) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [47, 49, 50, 51, 52] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]):
for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id)
# Create constraint that keeps the tool in the gripper
constraint = p.createConstraint(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id)
else:
# Disable collisions between the tool and robot
for j in (range(49, 64) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [25, 27, 28, 29, 30] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]):
for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id)
# Create constraint that keeps the tool in the gripper
constraint = p.createConstraint(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id)
p.changeConstraint(constraint, maxForce=500, physicsClientId=self.id)
return tool
def __init__(self, client, target=False):
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
if target==True:
f_name = os.path.join(os.path.dirname(__file__), 'tinyTarget.urdf')
self.humanoidAgent = p.loadURDF(
fileName=f_name,
basePosition=[0,1,0],
baseOrientation=p.getQuaternionFromEuler([1.57, 0, 0]),
globalScaling=0.25,
physicsClientId=client,
useFixedBase=True,
flags=(p.URDF_MAINTAIN_LINK_ORDER or p.URDF_USE_SELF_COLLISION or p.URDF_USE_SELF_COLLISION_INCLUDE_PARENT)
)
else:
f_name = os.path.join(os.path.dirname(__file__), 'tiny.urdf')
self.humanoidAgent = p.loadURDF(
fileName=f_name,
basePosition=[0,1,0],
baseOrientation=p.getQuaternionFromEuler([1.57, 0, 0]),
globalScaling=0.25,
physicsClientId=client,
useFixedBase=True,
flags=(p.URDF_MAINTAIN_LINK_ORDER or p.URDF_USE_SELF_COLLISION or p.URDF_USE_SELF_COLLISION_INCLUDE_PARENT)
)
self.numJoints=p.getNumJoints(self.humanoidAgent)
self.sphericalJoints=None
self.revoluteJoints=None
y2zPos = [0, 0, 0.0]
y2zOrn = p.getQuaternionFromEuler([1.57, 0, 0])
basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, [0,1,0], p.getQuaternionFromEuler([0, 0, 0]))
# set the agent's root position and orientation
p.resetBasePositionAndOrientation(
self.humanoidAgent,
posObj=basePos,
ornObj=baseOrn
)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setTimeStep(1./240)
self.initializeJoints()
def _get_observation(self):
goal_pos = p.getBasePositionAndOrientation(
self._obj, physicsClientId=self._client)
joint_p = self._get_joints_states()
cam_pos = self._robot.getLinkPosition("CameraBottom_optical_frame")
hand_pos = self._robot.getLinkPosition("l_hand")
# Object position relative to camera
inv = p.invertTransform(cam_pos[0], cam_pos[1])
rel_pos = p.multiplyTransforms(inv[0], inv[1], goal_pos[0],
goal_pos[1])
obj_rel_pos = np.array(rel_pos[0])
img = self._robot.getCameraFrame(self._cam)
if not detection.is_object_in_sight(img):
obj_rel_pos = np.array((0.0, 0.0, 0.0), dtype=np.float32)
return np.concatenate([
joint_p, cam_pos[0], cam_pos[1], hand_pos[0], hand_pos[1],
obj_rel_pos
]).astype(np.float32)
def capture_image(self):
# Camera
*_, camera_position, camera_orientation = p.getLinkState(
self.robot, self.camera_link)
camera_look_position, _ = p.multiplyTransforms(camera_position,
camera_orientation,
[0, 0.1, 0],
[0, 0, 0, 1])
view_matrix = p.computeViewMatrix(
cameraEyePosition=camera_position,
cameraTargetPosition=camera_look_position,
cameraUpVector=(0, 0, 1))
projection_matrix = p.computeProjectionMatrixFOV(fov=45.0,
aspect=1.0,
nearVal=0.1,
farVal=3.1)
return p.getCameraImage(300,
300,
view_matrix,
projection_matrix,
renderer=p.ER_BULLET_HARDWARE_OPENGL)[2]
def get_extended_observation(self):
self._observation = []
observation_lim = []
# ----------------------------------- #
# --- Robot and world observation --- #
# ----------------------------------- #
robot_observation, robot_obs_lim = self._robot.get_observation()
world_observation, world_obs_lim = self._world.get_observation()
self._observation.extend(list(robot_observation))
self._observation.extend(list(world_observation))
observation_lim.extend(robot_obs_lim)
observation_lim.extend(world_obs_lim)
# ----------------------------------------- #
# --- Object pose wrt hand c.o.m. frame --- #
# ----------------------------------------- #
inv_hand_pos, inv_hand_orn = p.invertTransform(
robot_observation[:3],
p.getQuaternionFromEuler(robot_observation[3:6]))
obj_pos_in_hand, obj_orn_in_hand = p.multiplyTransforms(
inv_hand_pos, inv_hand_orn, world_observation[:3],
p.getQuaternionFromEuler(world_observation[3:6]))
obj_euler_in_hand = p.getEulerFromQuaternion(obj_orn_in_hand)
self._observation.extend(list(obj_pos_in_hand))
self._observation.extend(list(obj_euler_in_hand))
observation_lim.extend([[-0.5, 0.5], [-0.5, 0.5], [-0.5, 0.5]])
observation_lim.extend([[0, 2 * m.pi], [0, 2 * m.pi], [0, 2 * m.pi]])
# ------------------- #
# --- Target pose --- #
# ------------------- #
self._observation.extend(self._target_pose)
observation_lim.extend(world_obs_lim[:3])
return np.array(self._observation), observation_lim
def move_eye_camera(robotID, x, y, z):
cam_link_state = p.getLinkState(robotID,
linkIndex=7,
computeForwardKinematics=1)
camera_pos_W = cam_link_state[-2]
camera_ort_W = cam_link_state[-1]
roll = 0.0 #this will cause end effector to rotate on its axis
pitch = 0.0
yaw = 0.0
cameraTargetPos = [x, y, z]
cameraTargetOrn = p.getQuaternionFromEuler([roll, pitch, yaw])
new_camera_pos_W, new_camera_ort_W = p.multiplyTransforms(
camera_pos_W, camera_ort_W, cameraTargetPos, cameraTargetOrn)
# print('new_camera_ort_W',new_camera_ort_W)
#following is the new function defined above in this file
accurateIK(robotID,
7,
new_camera_pos_W,
new_camera_ort_W,
useNullSpace=False)
return get_image(robotID)
def reset_state(self, q, dq):
"""Reset the robot to the desired states.
Args:
q (ndarray): Desired generalized positions.
dq (ndarray): Desired generalized velocities.
"""
vec2list = lambda m: np.array(m.T).reshape(-1).tolist()
if not self.useFixedBase:
# Get transform between inertial frame and link frame in base
base_stat = pybullet.getDynamicsInfo(self.robot_id, -1)
base_pos, base_quat = pybullet.multiplyTransforms(
vec2list(q[:3]), vec2list(q[3:7]), base_stat[3], base_stat[4])
pybullet.resetBasePositionAndOrientation(self.robot_id, base_pos,
base_quat)
# Pybullet assumes the base velocity to be aligned with the world frame.
rot = np.array(pybullet.getMatrixFromQuaternion(q[3:7])).reshape(
(3, 3))
pybullet.resetBaseVelocity(self.robot_id,
vec2list(rot.dot(dq[:3])),
vec2list(rot.dot(dq[3:6])))
for i, bullet_joint_id in enumerate(self.bullet_joint_ids):
pybullet.resetJointState(
self.robot_id,
bullet_joint_id,
q[5 + self.pinocchio_joint_ids[i]],
dq[4 + self.pinocchio_joint_ids[i]],
)
else:
for i, bullet_joint_id in enumerate(self.bullet_joint_ids):
pybullet.resetJointState(
self.robot_id,
bullet_joint_id,
q[self.pinocchio_joint_ids[i] - 1],
dq[self.pinocchio_joint_ids[i] - 1],
)
def generate_target(self):
# Randomly select either upper arm or forearm for the target limb to scratch
if self.gender == 'male':
self.limb, length, radius = [[5, 0.279, 0.043],
[7, 0.257,
0.033]][self.np_random.randint(2)]
else:
self.limb, length, radius = [[5, 0.264, 0.0355],
[7, 0.234,
0.027]][self.np_random.randint(2)]
self.target_on_arm = self.util.point_on_capsule(
p1=np.array([0, 0, 0]),
p2=np.array([0, 0, -length]),
radius=radius,
theta_range=(0, np.pi * 2))
arm_pos, arm_orient = p.getLinkState(self.human,
self.limb,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
target_pos, target_orient = p.multiplyTransforms(
arm_pos,
arm_orient,
self.target_on_arm, [0, 0, 0, 1],
physicsClientId=self.id)
sphere_collision = -1
sphere_visual = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.01,
rgbaColor=[0, 1, 1, 1],
physicsClientId=self.id)
self.target = p.createMultiBody(
baseMass=0.0,
baseCollisionShapeIndex=sphere_collision,
baseVisualShapeIndex=sphere_visual,
basePosition=target_pos,
useMaximalCoordinates=False,
physicsClientId=self.id)
self.update_targets()
def simTerminateCheck(self, robotPose):
termSim = False
# flipped robot termination criteria
if self.simulationParams["terminateIfFlipped"]:
upDir = p.multiplyTransforms([0, 0, 0], robotPose[1], [0, 0, 1],
[0, 0, 0, 1])[0]
if upDir[2] < 0:
termSim = True
# stuck robot terminate criteria
if self.stopMoveCount > self.simulationParams["maxStopMoveLength"]:
termSim = True
# boundary criteria
minZ = np.min(self.terrain.gridZ) - 1.
maxZ = np.max(self.terrain.gridZ) + 1.
minX = np.min(self.terrain.gridX) + 1.
maxX = np.max(self.terrain.gridX) - 1.
minY = np.min(self.terrain.gridY) + 1.
maxY = np.max(self.terrain.gridY) - 1.
if robotPose[0][0] > maxX or robotPose[0][0] < minX or \
robotPose[0][1] > maxY or robotPose[0][1] < minY or \
robotPose[0][2] > maxZ or robotPose[0][2] < minZ:
termSim = True
return termSim
def getExtendedObservation(self):
# get robot observation
self._observation = self._icub.getObservation()
# get object position and transform it wrt hand c.o.m. frame
objPos, objOrn = p.getBasePositionAndOrientation(self._objID)
objEuler = p.getEulerFromQuaternion(objOrn) #roll, pitch, yaw
self._observation.extend(list(objPos))
self._observation.extend(list(objEuler))
# relative object position wrt hand c.o.m. frame
invHandPos, invHandOrn = p.invertTransform(
self._observation[:3],
p.getQuaternionFromEuler(self._observation[3:6]))
objPosInHand, objOrnInHand = p.multiplyTransforms(
invHandPos, invHandOrn, objPos, objOrn)
objEulerInHand = p.getEulerFromQuaternion(objOrnInHand)
self._observation.extend(list(objPosInHand))
self._observation.extend(list(objEulerInHand))
return np.array(self._observation)
def get_base_sp_transform(self):
'''
Calculates pose of setpoint w.r.t. base frame.
Returns:
numpy.array: 6D pose of setpoint in base frame.
'''
state_ee = pb.getLinkState(self.robotId, self.baseLinkId, False, False,
self.clientId)
ee_pos_w, ee_ori_w = state_ee[4:6]
w_pos_ee, w_ori_ee = pb.invertTransform(ee_pos_w, ee_ori_w,
self.clientId)
state_sp = pb.getLinkState(self.spId, self.spGraspLinkId, False, False,
self.clientId)
sp_pos_w, sp_ori_w = state_sp[4:6]
sp_pos_ee, sp_ori_ee = pb.multiplyTransforms(w_pos_ee, w_ori_ee,
sp_pos_w, sp_ori_w,
self.clientId)
sp_eul_ee = pb.getEulerFromQuaternion(sp_ori_ee, self.clientId)
return np.array(sp_pos_ee + sp_eul_ee)
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink,
physicsClientId):
dyn = p.getDynamicsInfo(bodyUid,
linkIndex,
physicsClientId=physicsClientId)
urdfLink.urdf_inertial.mass = dyn[0]
urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
#todo
urdfLink.urdf_inertial.origin_xyz = dyn[3]
rpy = p.getEulerFromQuaternion(dyn[4])
urdfLink.urdf_inertial.origin_rpy = rpy
visualShapes = p.getVisualShapeData(bodyUid,
physicsClientId=physicsClientId)
matIndex = 0
for v in visualShapes:
if (v[1] == linkIndex):
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
if (v[2] == p.GEOM_BOX):
urdfVisual.geom_extents = v[3]
if (v[2] == p.GEOM_SPHERE):
urdfVisual.geom_radius = v[3][0]
if (v[2] == p.GEOM_MESH):
urdfVisual.geom_meshfilename = v[4].decode("utf-8")
urdfVisual.geom_meshscale = v[3]
if (v[2] == p.GEOM_CYLINDER):
urdfVisual.geom_length = v[3][0]
urdfVisual.geom_radius = v[3][1]
if (v[2] == p.GEOM_CAPSULE):
urdfVisual.geom_length = v[3][0]
urdfVisual.geom_radius = v[3][1]
urdfVisual.origin_xyz = v[5]
urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
urdfVisual.material_rgba = v[7]
name = 'mat_{}_{}'.format(linkIndex, matIndex)
urdfVisual.material_name = name
urdfLink.urdf_visual_shapes.append(urdfVisual)
matIndex = matIndex + 1
collisionShapes = p.getCollisionShapeData(
bodyUid, linkIndex, physicsClientId=physicsClientId)
for v in collisionShapes:
urdfCollision = UrdfCollision()
urdfCollision.geom_type = v[2]
if (v[2] == p.GEOM_BOX):
urdfCollision.geom_extents = v[3]
if (v[2] == p.GEOM_SPHERE):
urdfCollision.geom_radius = v[3][0]
if (v[2] == p.GEOM_MESH):
urdfCollision.geom_meshfilename = v[4].decode("utf-8")
urdfCollision.geom_meshscale = v[3]
if (v[2] == p.GEOM_CYLINDER):
urdfCollision.geom_length = v[3][0]
urdfCollision.geom_radius = v[3][1]
if (v[2] == p.GEOM_CAPSULE):
urdfCollision.geom_length = v[3][0]
urdfCollision.geom_radius = v[3][1]
pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
v[5], v[6])
urdfCollision.origin_xyz = pos
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
urdfLink.urdf_collision_shapes.append(urdfCollision)
def joinUrdf(self,
childEditor,
parentLinkIndex=0,
jointPivotXYZInParent=[0, 0, 0],
jointPivotRPYInParent=[0, 0, 0],
jointPivotXYZInChild=[0, 0, 0],
jointPivotRPYInChild=[0, 0, 0],
parentPhysicsClientId=0,
childPhysicsClientId=0):
childLinkIndex = len(self.urdfLinks)
insertJointIndex = len(self.urdfJoints)
#combine all links, and add a joint
for link in childEditor.urdfLinks:
self.linkNameToIndex[link.link_name] = len(self.urdfLinks)
self.urdfLinks.append(link)
for joint in childEditor.urdfJoints:
self.urdfJoints.append(joint)
#add a new joint between a particular
jointPivotQuatInChild = p.getQuaternionFromEuler(jointPivotRPYInChild)
invJointPivotXYZInChild, invJointPivotQuatInChild = p.invertTransform(
jointPivotXYZInChild, jointPivotQuatInChild)
#apply this invJointPivot***InChild to all inertial, visual and collision element in the child link
#inertial
pos, orn = p.multiplyTransforms(
self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz,
p.getQuaternionFromEuler(
self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy),
invJointPivotXYZInChild,
invJointPivotQuatInChild,
physicsClientId=parentPhysicsClientId)
self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz = pos
self.urdfLinks[
childLinkIndex].urdf_inertial.origin_rpy = p.getEulerFromQuaternion(
orn)
#all visual
for v in self.urdfLinks[childLinkIndex].urdf_visual_shapes:
pos, orn = p.multiplyTransforms(
v.origin_xyz, p.getQuaternionFromEuler(v.origin_rpy),
invJointPivotXYZInChild, invJointPivotQuatInChild)
v.origin_xyz = pos
v.origin_rpy = p.getEulerFromQuaternion(orn)
#all collision
for c in self.urdfLinks[childLinkIndex].urdf_collision_shapes:
pos, orn = p.multiplyTransforms(
c.origin_xyz, p.getQuaternionFromEuler(c.origin_rpy),
invJointPivotXYZInChild, invJointPivotQuatInChild)
c.origin_xyz = pos
c.origin_rpy = p.getEulerFromQuaternion(orn)
childLink = self.urdfLinks[childLinkIndex]
parentLink = self.urdfLinks[parentLinkIndex]
joint = UrdfJoint()
joint.link = childLink
joint.joint_name = "joint_dummy1"
joint.joint_type = p.JOINT_REVOLUTE
joint.joint_lower_limit = 0
joint.joint_upper_limit = -1
joint.parent_name = parentLink.link_name
joint.child_name = childLink.link_name
joint.joint_origin_xyz = jointPivotXYZInParent
joint.joint_origin_rpy = jointPivotRPYInParent
joint.joint_axis_xyz = [0, 0, 1]
#the following commented line would crash PyBullet, it messes up the joint indexing/ordering
#self.urdfJoints.append(joint)
#so make sure to insert the joint in the right place, to links/joints match
self.urdfJoints.insert(insertJointIndex, joint)
return joint
def initializeFromBulletBody(self, bodyUid, physicsClientId):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink,
physicsClientId)
baseLink.link_name = p.getBodyInfo(
bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
self.linkNameToIndex[baseLink.link_name] = len(self.urdfLinks)
self.urdfLinks.append(baseLink)
#optionally create child links and joints
for j in range(p.getNumJoints(bodyUid,
physicsClientId=physicsClientId)):
jointInfo = p.getJointInfo(bodyUid,
j,
physicsClientId=physicsClientId)
urdfLink = UrdfLink()
self.convertLinkFromMultiBody(bodyUid, j, urdfLink,
physicsClientId)
urdfLink.link_name = jointInfo[12].decode("utf-8")
self.linkNameToIndex[urdfLink.link_name] = len(self.urdfLinks)
self.urdfLinks.append(urdfLink)
urdfJoint = UrdfJoint()
urdfJoint.link = urdfLink
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
urdfJoint.joint_type = jointInfo[2]
urdfJoint.joint_axis_xyz = jointInfo[13]
orgParentIndex = jointInfo[16]
if (orgParentIndex < 0):
urdfJoint.parent_name = baseLink.link_name
else:
parentJointInfo = p.getJointInfo(
bodyUid, orgParentIndex, physicsClientId=physicsClientId)
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dynChild = p.getDynamicsInfo(bodyUid,
j,
physicsClientId=physicsClientId)
childInertiaPos = dynChild[3]
childInertiaOrn = dynChild[4]
parentCom2JointPos = jointInfo[14]
parentCom2JointOrn = jointInfo[15]
tmpPos, tmpOrn = p.multiplyTransforms(childInertiaPos,
childInertiaOrn,
parentCom2JointPos,
parentCom2JointOrn)
tmpPosInv, tmpOrnInv = p.invertTransform(tmpPos, tmpOrn)
dynParent = p.getDynamicsInfo(bodyUid,
orgParentIndex,
physicsClientId=physicsClientId)
parentInertiaPos = dynParent[3]
parentInertiaOrn = dynParent[4]
pos, orn = p.multiplyTransforms(parentInertiaPos, parentInertiaOrn,
tmpPosInv, tmpOrnInv)
pos, orn_unused = p.multiplyTransforms(parentInertiaPos,
parentInertiaOrn,
parentCom2JointPos,
[0, 0, 0, 1])
urdfJoint.joint_origin_xyz = pos
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
self.urdfJoints.append(urdfJoint)
def dot(self, pose: 'Pose'):
vector, quaternion = pybullet.multiplyTransforms(
self.vector, self.quaternion, pose.vector, pose.quaternion)
return Pose(vector, quaternion)
def visualize(data):
print(data.shape)
print("test", data.iloc[0, 2])
p.connect(p.GUI)
p.resetDebugVisualizerCamera(cameraDistance=3,
cameraYaw=30,
cameraPitch=-52,
cameraTargetPosition=[0, 0, 0])
p.resetSimulation()
p.setGravity(0, 0, -9.81)
p.setTimeStep(0.01)
planeId = p.loadURDF("plane.xml")
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
baxterStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
botId = p.loadURDF("baxter.xml", [0, 0, 0],
baxterStartOrientation,
useFixedBase=1)
p.setJointMotorControlArray(
botId,
jointIndices=[13, 14, 15, 16, 17, 19, 20, 36, 37, 38, 39, 40, 42, 43],
controlMode=p.POSITION_CONTROL,
targetPositions=[
0.75, -0.9, 0, 1.8, 0, -0.9, 0, -0.75, -0.9, 0, 1.8, 0, -0.9, 0
])
for i in range(10):
p.stepSimulation()
p.setJointMotorControlArray(botId,
jointIndices=[29, 31, 52, 54],
controlMode=p.POSITION_CONTROL,
targetPositions=[0.75] * 4,
forces=[10000] * 4)
p.setJointMotorControlArray(botId,
jointIndices=[20, 43],
controlMode=p.POSITION_CONTROL,
targetPositions=[1.8, 1.8])
for i in range(10):
p.stepSimulation()
revoluteJoints = getRevoluteJoints(botId)
count = 0
for i in range(10):
gripperPosition1 = p.calculateInverseKinematics(
botId,
28,
[data.iloc[count, 0], data.iloc[count, 1], data.iloc[count, 2]], [
data.iloc[count, 3], data.iloc[count, 4], data.iloc[count, 5],
data.iloc[count, 6]
],
maxNumIterations=1000)
gripperPosition2 = p.calculateInverseKinematics(
botId,
51,
[data.iloc[count, 7], data.iloc[count, 8], data.iloc[count, 9]], [
data.iloc[count, 10], data.iloc[count, 11],
data.iloc[count, 12], data.iloc[count, 13]
],
maxNumIterations=1000)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[0:10],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition1[0:10],
forces=[100] * 10)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[10:],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition2[10:],
forces=[100] * 9)
for j in range(10):
p.stepSimulation()
count += 1
for i in range(100):
p.stepSimulation()
tableId1 = p.loadURDF("table_dock.xml",
[1 + data.iloc[0, 14], 0.1 + data.iloc[0, 15], 0],
cubeStartOrientation,
globalScaling=0.75)
for i in range(100):
p.stepSimulation()
p.setJointMotorControlArray(botId,
jointIndices=[29, 31, 52, 54],
controlMode=p.POSITION_CONTROL,
targetPositions=[-0.55] * 4,
forces=[10000] * 4)
for i in range(100):
p.stepSimulation()
for i in range(10):
gripperPosition1 = p.calculateInverseKinematics(
botId,
28,
[data.iloc[count, 0], data.iloc[count, 1], data.iloc[count, 2]], [
data.iloc[count, 3], data.iloc[count, 4], data.iloc[count, 5],
data.iloc[count, 6]
],
maxNumIterations=1000)
gripperPosition2 = p.calculateInverseKinematics(
botId,
51,
[data.iloc[count, 7], data.iloc[count, 8], data.iloc[count, 9]], [
data.iloc[count, 10], data.iloc[count, 11],
data.iloc[count, 12], data.iloc[count, 13]
],
maxNumIterations=1000)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[0:10],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition1[0:10],
forces=[100] * 10)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[10:],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition2[10:],
forces=[100] * 9)
for i in range(10):
p.stepSimulation()
count += 1
for i in range(100):
p.stepSimulation()
p.setJointMotorControlArray(botId,
jointIndices=[29, 31, 52, 54],
controlMode=p.POSITION_CONTROL,
targetPositions=[0.75] * 4,
forces=[10000] * 4)
for i in range(100):
p.stepSimulation()
inertpos, inertorientation = p.invertTransform(
p.getDynamicsInfo(tableId1, -1)[3],
p.getDynamicsInfo(tableId1, -1)[4])
table1pos, table1orientation = p.multiplyTransforms(
p.getBasePositionAndOrientation(tableId1)[0],
p.getBasePositionAndOrientation(tableId1)[1], inertpos,
inertorientation)
for i in range(100):
p.stepSimulation()
for i in range(40):
gripperPosition1 = p.calculateInverseKinematics(
botId,
28,
[data.iloc[count, 0], data.iloc[count, 1], data.iloc[count, 2]], [
data.iloc[count, 3], data.iloc[count, 4], data.iloc[count, 5],
data.iloc[count, 6]
],
maxNumIterations=1000)
gripperPosition2 = p.calculateInverseKinematics(
botId,
51,
[data.iloc[count, 7], data.iloc[count, 8], data.iloc[count, 9]], [
data.iloc[count, 10], data.iloc[count, 11],
data.iloc[count, 12], data.iloc[count, 13]
],
maxNumIterations=1000)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[0:10],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition1[0:10],
forces=[100] * 10)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[10:],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition2[10:],
forces=[100] * 9)
for j in range(10):
p.stepSimulation()
count += 1
for i in range(100):
p.stepSimulation()
p.removeBody(tableId1)
tableId1 = p.loadURDF("table_dock.xml",
table1pos,
table1orientation,
globalScaling=0.75)
tableId2 = p.loadURDF("table_peg.xml",
[1 + data.iloc[0, 16], -0.1 + data.iloc[0, 17], 0],
cubeStartOrientation,
globalScaling=0.75)
for i in range(100):
p.stepSimulation()
p.setJointMotorControlArray(botId,
jointIndices=[29, 31, 52, 54],
controlMode=p.POSITION_CONTROL,
targetPositions=[-0.75] * 4,
forces=[10000] * 4)
for i in range(1000):
p.stepSimulation()
for i in range(10):
gripperPosition1 = p.calculateInverseKinematics(
botId,
28,
[data.iloc[count, 0], data.iloc[count, 1], data.iloc[count, 2]], [
data.iloc[count, 3], data.iloc[count, 4], data.iloc[count, 5],
data.iloc[count, 6]
],
maxNumIterations=1000)
gripperPosition2 = p.calculateInverseKinematics(
botId,
51,
[data.iloc[count, 7], data.iloc[count, 8], data.iloc[count, 9]], [
data.iloc[count, 10], data.iloc[count, 11],
data.iloc[count, 12], data.iloc[count, 13]
],
maxNumIterations=1000)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[0:10],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition1[0:10],
forces=[100] * 10)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[10:],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition2[10:],
forces=[100] * 9)
p.setJointMotorControlArray(botId,
jointIndices=[29, 31, 52, 54],
controlMode=p.POSITION_CONTROL,
targetPositions=[-0.55] * 4,
forces=[10000] * 4)
for i in range(10):
p.stepSimulation()
count += 1
for i in range(100):
p.stepSimulation()
p.setJointMotorControlArray(botId,
jointIndices=[29, 31, 52, 54],
controlMode=p.POSITION_CONTROL,
targetPositions=[1.2] * 4,
forces=[10000] * 4)
for i in range(100):
p.stepSimulation()
inertpos, inertorientation = p.invertTransform(
p.getDynamicsInfo(tableId2, -1)[3],
p.getDynamicsInfo(tableId2, -1)[4])
table2pos, table2orientation = p.multiplyTransforms(
p.getBasePositionAndOrientation(tableId2)[0],
p.getBasePositionAndOrientation(tableId2)[1], inertpos,
inertorientation)
for i in range(100):
p.stepSimulation()
for i in range(40):
gripperPosition1 = p.calculateInverseKinematics(
botId,
28,
[data.iloc[count, 0], data.iloc[count, 1], data.iloc[count, 2]], [
data.iloc[count, 3], data.iloc[count, 4], data.iloc[count, 5],
data.iloc[count, 6]
],
maxNumIterations=1000)
gripperPosition2 = p.calculateInverseKinematics(
botId,
51,
[data.iloc[count, 7], data.iloc[count, 8], data.iloc[count, 9]], [
data.iloc[count, 10], data.iloc[count, 11],
data.iloc[count, 12], data.iloc[count, 13]
],
maxNumIterations=1000)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[0:10],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition1[0:10],
forces=[100] * 10)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[10:],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition2[10:],
forces=[100] * 9)
for i in range(10):
p.stepSimulation()
count += 1
for i in range(100):
p.stepSimulation()
p.removeBody(tableId2)
tableId2 = p.loadURDF("table_peg.xml",
table2pos,
table2orientation,
globalScaling=0.75)
for i in range(100):
p.stepSimulation()
p.setJointMotorControlArray(botId,
jointIndices=[29, 31, 52, 54],
controlMode=p.POSITION_CONTROL,
targetPositions=[-0.75] * 4,
forces=[10000] * 4)
for i in range(100):
p.stepSimulation()
for i in range(20):
gripperPosition1 = p.calculateInverseKinematics(
botId,
28,
[data.iloc[count, 0], data.iloc[count, 1], data.iloc[count, 2]], [
data.iloc[count, 3], data.iloc[count, 4], data.iloc[count, 5],
data.iloc[count, 6]
],
maxNumIterations=1000)
gripperPosition2 = p.calculateInverseKinematics(
botId,
51,
[data.iloc[count, 7], data.iloc[count, 8], data.iloc[count, 9]], [
data.iloc[count, 10], data.iloc[count, 11],
data.iloc[count, 12], data.iloc[count, 13]
],
maxNumIterations=1000)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[0:10],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition1[0:10],
forces=[100] * 10)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[10:],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition2[10:],
forces=[100] * 9)
p.setJointMotorControlArray(botId,
jointIndices=[29, 31, 52, 54],
controlMode=p.POSITION_CONTROL,
targetPositions=[-0.75] * 4,
forces=[10000] * 4)
count += 1
for i in range(10):
p.stepSimulation()
for i in range(100):
p.stepSimulation()
if p.getBasePositionAndOrientation(tableId1)[0][
2] > 0.4 and p.getBasePositionAndOrientation(tableId2)[0][2] > 0.4:
label = 1
else:
label = 0
if abs(table1orientation[2] - table2orientation[2]) < 0.05:
orientation = 1
else:
orientation = 0
p.disconnect()
return label, orientation
def pbvs(self,
arm,
goal_pos,
goal_rot,
kv=1.3,
kw=0.8,
eps_pos=0.005,
eps_rot=0.05,
vs_rate=20,
plot_pose=False,
print_err=False,
perturb_jacobian=False,
perturb_Jac_joint_mu=0.1,
perturb_Jac_joint_sigma=0.1,
perturb_orientation=False,
mu_R=0.4,
sigma_R=0.4,
perturb_motion_R_mu=0.0,
perturb_motion_R_sigma=0.0,
perturb_motion_t_mu=0.0,
perturb_motion_t_sigma=0.0,
plot_result=False,
pf_mode=False):
# Initialize variables
self.perturb_Jac_joint_mu = perturb_Jac_joint_mu
self.perturb_Jac_joint_sigma = perturb_Jac_joint_sigma
self.perturb_R_mu = mu_R
self.perturb_R_sigma = sigma_R
if arm == "right":
tool = self.right_tool
elif arm == "left":
tool = self.left_tool
else:
print('''arm incorrect, input "left" or "right" ''')
return
# Initialize pos_plot_id for later use
if pf_mode:
cur_pos, cur_rot = SE32_trans_rot(self.cur_pose_est)
else:
cur_pos, cur_rot = SE32_trans_rot(
self.Trc.inverse() * get_pose(self.robot, tool, SE3=True))
if plot_pose:
pos_plot_id = self.draw_pose_cam(trans_rot2SE3(cur_pos, cur_rot))
# record end effector pose and time stamp at each iteration
if plot_result:
start = time.time()
times = []
eef_pos = []
eef_rot = []
if pf_mode:
motion = sp.SE3()
##### Control loop starts #####
while True:
# If using particle filter and the hog computation has finished, exit pbvs control
# and assign total estimated motion in eef frame
if pf_mode and self.shared_pf_fin.value == 1:
# eef Motion estimated from pose estimate of last iteration and current fk
motion_truth = (self.Trc *
self.cur_pose_est).inverse() * get_pose(
self.robot, tool, SE3=True)
# print("motion truth:", motion_truth)
dR = sp.SO3.exp(
np.random.normal(perturb_motion_R_mu,
perturb_motion_R_sigma, 3)).matrix()
dt = np.random.normal(perturb_motion_t_mu,
perturb_motion_t_sigma, 3)
# self.draw_pose_cam(motion)
self.motion = motion_truth * sp.SE3(dR, dt)
return
# get current eef pose in camera frame
if pf_mode:
cur_pos, cur_rot = SE32_trans_rot(self.cur_pose_est)
else:
cur_pos, cur_rot = SE32_trans_rot(
self.Trc.inverse() * get_pose(self.robot, tool, SE3=True))
if plot_pose:
self.draw_pose_cam(trans_rot2SE3(cur_pos, cur_rot),
uids=pos_plot_id)
if plot_result:
eef_pos.append(cur_pos)
eef_rot.append(cur_rot)
times.append(time.time() - start)
cur_pos_inv, cur_rot_inv = p.invertTransform(cur_pos, cur_rot)
# Pose of goal in camera frame
pos_cg, rot_cg = p.multiplyTransforms(cur_pos_inv, cur_rot_inv,
goal_pos, goal_rot)
# Evaluate current translational and rotational error
err_pos = np.linalg.norm(pos_cg)
err_rot = np.linalg.norm(p.getAxisAngleFromQuaternion(rot_cg)[1])
if err_pos < eps_pos and err_rot < eps_rot:
p.removeAllUserDebugItems()
break
else:
if print_err:
print("Error to goal, position: {:2f}, orientation: {:2f}".
format(err_pos, err_rot))
Rsc = np.array(p.getMatrixFromQuaternion(cur_rot)).reshape(3, 3)
# Perturb Rsc in SO(3) by a random variable in tangent space so(3)
if perturb_orientation:
dR = sp.SO3.exp(
np.random.normal(self.perturb_R_mu, self.perturb_R_sigma,
3))
Rsc = Rsc.dot(dR.matrix())
twist_local = np.hstack(
(np.array(pos_cg) * kv,
np.array(quat2se3(rot_cg)) * kw)).reshape(6, 1)
local2global = np.block([[Rsc, np.zeros((3, 3))],
[np.zeros((3, 3)), Rsc]])
twist_global = local2global.dot(twist_local)
start_loop = time.time()
self.cartesian_vel_control(arm,
np.asarray(twist_global),
1 / vs_rate,
perturb_jacobian=perturb_jacobian)
if pf_mode:
delta_t = time.time() - start_loop
motion = motion * sp.SE3.exp(twist_local * delta_t)
# self.draw_pose_cam(motion)
self.goal_reached = True
print("PBVS goal achieved!")
if plot_result:
eef_pos = np.array(eef_pos)
eef_rot_rpy = np.array(
[p.getEulerFromQuaternion(quat) for quat in eef_rot])
fig, axs = plt.subplots(3, 2)
sub_titles = [['x', 'roll'], ['y', 'pitch'], ['z', 'yaw']]
fig.suptitle(
"Position Based Visual Servo End Effector Pose - time plot")
for i in range(3):
axs[i, 0].plot(times, eef_pos[:, i] * 100)
axs[i, 0].plot(times, goal_pos[i] * np.ones(len(times)) * 100)
axs[i, 0].legend([sub_titles[i][0], 'goal'])
axs[i, 0].set_xlabel('Time(s)')
axs[i, 0].set_ylabel('cm')
goal_rpy = p.getEulerFromQuaternion(goal_rot)
for i in range(3):
axs[i, 1].plot(times, eef_rot_rpy[:, i] * 180 / np.pi)
axs[i, 1].plot(times,
goal_rpy[i] * np.ones(len(times)) * 180 / np.pi)
axs[i, 1].legend([sub_titles[i][1], 'goal'])
axs[i, 1].set_xlabel('Time(s)')
axs[i, 1].set_ylabel('deg')
for ax in axs.flat:
ax.set(xlabel='time')
plt.show()
p.setGravity(0,0,-10)
p.setRealTimeSimulation(0)
#coordPos = [0,0,0]
#coordOrnEuler = [0,0,0]
#coordPos= [0.7000000000000004, 0, 0.22000000000000006]
#coordOrnEuler= [0, -0.2617993877991496, 0]
coordPos= [0.07, 0, -0.6900000000000004]
coordOrnEuler= [0, 0, 0]
coordPos2= [0, 0, 0 ]
coordOrnEuler2= [0, 0, 0]
invPos,invOrn=p.invertTransform(coordPos,p.getQuaternionFromEuler(coordOrnEuler))
mPos,mOrn = p.multiplyTransforms(invPos,invOrn, coordPos2,p.getQuaternionFromEuler(coordOrnEuler2))
eul = p.getEulerFromQuaternion(mOrn)
print("rpy=\"",eul[0],eul[1],eul[2],"\" xyz=\"", mPos[0],mPos[1], mPos[2])
shift=0
gui = 1
frame=0
states=[]
states.append(p.saveState())
#observations=[]
#observations.append(obs)
running = True
#print("duration=",frameData[0])
#print(pos=[frameData])
basePos1Start = [frameData[1],frameData[2],frameData[3]]
basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]),
basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]),
basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
#pre-rotate to make z-up
if (useZUp):
y2zPos=[0,0,0.0]
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
y2zPos=[0,2,0.0]
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
p.resetBasePositionAndOrientation(humanoid2, basePos,baseOrn)
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
neckRot = p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)