Python pybullet.getEulerFromQuaternion() Examples

def get_observation(self):
        observation = []
        observation_lim = []

        # get object position
        obj_pos, obj_orn = p.getBasePositionAndOrientation(self.obj_id, physicsClientId=self._physics_client_id)
        observation.extend(list(obj_pos))
        observation_lim.extend(self._ws_lim)

        if self._control_eu_or_quat is 0:
            obj_euler = p.getEulerFromQuaternion(obj_orn)  # roll, pitch, yaw
            observation.extend(list(obj_euler))
            observation_lim.extend([[-m.pi, m.pi], [-m.pi, m.pi], [-m.pi, m.pi]])
        else:
            observation.extend(list(obj_orn))
            observation_lim.extend([[-1, 1], [-1, 1], [-1, 1], [-1, 1]])

        return observation, observation_lim 

def rpy(self):
        return p.getEulerFromQuaternion(self.body_part.get_orientation()) 

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]])

        return np.array(self._observation), observation_lim 

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._tg_pose)
        observation_lim.extend(world_obs_lim[:3])

        return np.array(self._observation), observation_lim 

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]])

        return np.array(self._observation), observation_lim 

def _compute_observation(self):
        cubePos, cubeOrn = p.getBasePositionAndOrientation(self.botId)
        cubeEuler = p.getEulerFromQuaternion(cubeOrn)
        linear, angular = p.getBaseVelocity(self.botId)
        return [cubeEuler[0],angular[0],self.vt] 

def _initProcess(self):
        """
        INTERNAL METHOD, initialize the motion process and all variables
        needed.
        """
        # Get actual position in frame world
        self.pose_init["position"], self.pose_init["orientation"] =\
            pybullet.getBasePositionAndOrientation(
                self.robot_model,
                physicsClientId=self.physics_client)

        # convert pose_init orientation in orn_euler
        self.pose_init["orientation"] = pybullet.getEulerFromQuaternion(
            self.pose_init["orientation"]
        )
        self._updateGoal()
        self._updateConstraint()

        # Compute the ratio distance requested on the total distance
        distance = getDistance(
            self.pose_init["position"],
            self.pose_goal["position"])

        self.p_x = 0
        self.p_y = 0
        self.p_theta = 0

        if distance:
            self.p_x = (
                self.pose_goal["position"][0] -
                self.pose_init["position"][0]) / distance
            self.p_y = (
                self.pose_goal["position"][1] -
                self.pose_init["position"][1]) / distance

        theta_to_do = getOrientation(
            self.pose_init["orientation"],
            self.pose_goal["orientation"])

        if abs(theta_to_do):
            self.p_theta = abs(theta_to_do) / theta_to_do 

def move(self, x, y, theta):
        """
        Apply a speed on the robot's base.

        Parameters:
            x - Speed on the x axis, in m/s
            y - Speed on the y axis, in m/s
            theta - Rotational speed around the z axis, in rad/s
        """
        # Kill any previous moveTo process running
        self.moveTo(0, 0, 0, frame=BaseController.FRAME_ROBOT, _async=True)

        # Bound the velocity. The max acceleration is not taken into account
        # here, this is a potential improvment
        if abs(x) > PepperBaseController.MAX_LINEAR_VELOCITY:
            x = PepperBaseController.MAX_LINEAR_VELOCITY * (x/abs(x))
        if abs(y) > PepperBaseController.MAX_LINEAR_VELOCITY:
            y = PepperBaseController.MAX_LINEAR_VELOCITY * (y/abs(y))
        if abs(theta) > PepperBaseController.MAX_ANGULAR_VELOCITY:
            theta = PepperBaseController.MAX_ANGULAR_VELOCITY *\
                (theta/abs(theta))

        actual_pos, actual_orn = pybullet.getBasePositionAndOrientation(
            self.robot_model,
            physicsClientId=self.physics_client)

        # convert actual_orn into euler
        actual_orn = pybullet.getEulerFromQuaternion(actual_orn)

        linear_world_velocity = [
            x * math.cos(actual_orn[2]) - y * math.sin(actual_orn[2]),
            x * math.sin(actual_orn[2]) + y * math.cos(actual_orn[2]),
            0]

        time.sleep(0.02)
        pybullet.resetBaseVelocity(
            self.robot_model,
            linear_world_velocity,
            [0, 0, theta],
            physicsClientId=self.physics_client) 

def _updateGoal(self):
        """
        INTERNAL METHOD, update the position of the goal.
        """
        # get actual position in frame world
        actual_pos, actual_orn = pybullet.getBasePositionAndOrientation(
            self.robot_model,
            physicsClientId=self.physics_client)

        x, y, theta = self.goal
        # pose x, y, z
        pose_requested = [x, y, 0]

        # orientation requested (euler)
        orn_requested = [0, 0, theta]
        # if we are in frame robot express the position in the frame world
        if self.frame == BaseController.FRAME_ROBOT:
            orn_euler = pybullet.getEulerFromQuaternion(actual_orn)
            pose_requested = [
                pose_requested[0] * math.cos(orn_euler[2])
                - pose_requested[1] * math.sin(orn_euler[2])
                + actual_pos[0],
                pose_requested[0] * math.sin(orn_euler[2])
                + pose_requested[1] * math.cos(orn_euler[2])
                + actual_pos[1],
                0]
            orn_requested = [
                orn_euler[0],
                orn_euler[1],
                orn_euler[2] + theta]
        self.pose_goal["position"] = pose_requested
        self.pose_goal["orientation"] = orn_requested 

def getPosition(self):
        """
        Gets the position of the robot's base in the world frame.

        Returns:
            x - The position of the robot's base on the x axis, in meters
            y - The positions of the robot's base on the y axis in meters
            theta - The rotation of the robot's base on the z axis in meters
        """
        position, quaternions = pybullet.getBasePositionAndOrientation(
            self.robot_model,
            physicsClientId=self.physics_client)

        theta = pybullet.getEulerFromQuaternion(quaternions)[2]
        return position[0], position[1], theta 

def update(self):
        linkPose = np.zeros(7)
        for i in range(-1,self.num_links):
            linkPose = self.state_fields_of_pose_of(i)
            vp = linkPose[:3] - self.pose[i][:3]
            av = p.getEulerFromQuaternion(linkPose[3:])
            oav = p.getEulerFromQuaternion(self.pose[i][3:])
            vo = np.subtract(av,oav)
            self.velocity[i+1] = np.concatenate((vp,vo),axis=0)
            self.pose[i+1] = linkPose 

def rpy(self):
		return p.getEulerFromQuaternion(self.body_part.current_orientation()) 

def rpy(self):
        return p.getEulerFromQuaternion(self.body_part.get_orientation()) 

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 getFrames(self):
        """Gets the available frames in the current robot model
        
        Returns:
            dict -- dict of str -> (pos, orientation)
        """
        frames = {}

        for name in self.frames.keys():
            jointState = p.getLinkState(self.robot, self.frames[name])
            pos = jointState[0]
            orientation = p.getEulerFromQuaternion(jointState[1])
            frames[name] = [pos, orientation]

        return frames 

def getObservation(self):
    observation = []
    state = p.getLinkState(self.kukaUid,self.kukaGripperIndex)
    pos = state[0]
    orn = state[1]
    euler = p.getEulerFromQuaternion(orn)
        
    observation.extend(list(pos))
    observation.extend(list(euler))
    
    return observation 

def rpy(self):
		return pybullet.getEulerFromQuaternion(self.body_part.current_orientation()) 

def updateImuMeasurments(self):
        """Get IMU measurements from simulation and convert to usable format"""
        imu_info = p.getLinkState(self.body, self.imu, computeLinkVelocity=1)
        self.imuMeasurements.position = imu_info[0]
        self.imuMeasurements.orientation = p.getEulerFromQuaternion(imu_info[1])
        self.imuMeasurements.velocity = imu_info[6] 

def getRobotPose(self):
        """Gets the robot (origin) position

        Returns:
            (tuple(3), tuple(3)) -- (x,y,z), (roll, pitch, yaw)
        """
        pose = p.getBasePositionAndOrientation()
        return (pose[0], p.getEulerFromQuaternion(pose[1])) 

def apply_action(self, a):
        yaw= a
        pos, ornQuaternion = p.getBasePositionAndOrientation(self.robot_ids[0])
        ornEuler = p.getEulerFromQuaternion(ornQuaternion)
        currAngle = ornEuler[2]-self.config['yaw_constant']*yaw
        print(pos)
        print(ornEuler)
        pos = [pos[0]+ self.config['vx_constant']*self.vx*math.cos(currAngle)+random.uniform(self.config['wind_limits'][0], self.config['wind_limits'][1]), 
        pos[1]+self.config['vx_constant']*self.vx*math.sin(currAngle)+random.uniform(self.config['wind_limits'][0], self.config['wind_limits'][1]), self.height]
        pitch = min(max( ornEuler[0] + random.uniform(self.config['d_roll_per_step'][0], self.config['d_roll_per_step'][1]), self.config['roll_limits'][0]), self.config['roll_limits'][1])
        ornEuler = [pitch, ornEuler[1], currAngle]
        ornQuaternion = p.getQuaternionFromEuler(ornEuler)
        self.robot_body.set_pose(pos, ornQuaternion)
        return pos[0], pos[1], ornEuler[2], self.height, self.vx 

def euler_from_quat(quat):
        quat = list(quat)
        euler = p.getEulerFromQuaternion(quat)
        return np.array(euler, dtype=np.float32) 

def get_body_euler(body):
        _, quat = p.getBasePositionAndOrientation(body)
        euler = p.getEulerFromQuaternion(quat)
        return np.array(euler, dtype=np.float32) 

def get_cstr_dof(cstr):
        _, _, _, _, _, _, _, pos, _, quat, max_force = p.getConstraintInfo(cstr)
        pos = np.array(pos, dtype=np.float32)
        euler = p.getEulerFromQuaternion(quat)
        euler = np.array(euler, dtype=np.float32)
        return pos, euler 

def step(self, action):
        self.take_step(action, robot_arm='right', gains=self.config('robot_gains'), forces=self.config('robot_forces'), human_gains=0.0005)

        robot_force_on_human, cup_force_on_human = self.get_total_force()
        total_force_on_human = robot_force_on_human + cup_force_on_human
        reward_water, water_mouth_velocities, water_hit_human_reward = self.get_water_rewards()
        end_effector_velocity = np.linalg.norm(p.getBaseVelocity(self.cup, physicsClientId=self.id)[0])
        obs = self._get_obs([cup_force_on_human], [robot_force_on_human, cup_force_on_human])

        # Get human preferences
        preferences_score = self.human_preferences(end_effector_velocity=end_effector_velocity, total_force_on_human=robot_force_on_human, tool_force_at_target=cup_force_on_human, food_hit_human_reward=water_hit_human_reward, food_mouth_velocities=water_mouth_velocities)

        cup_pos, cup_orient = p.getBasePositionAndOrientation(self.cup, physicsClientId=self.id)
        cup_pos, cup_orient = p.multiplyTransforms(cup_pos, cup_orient, [0, 0.06, 0], p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
        cup_top_center_pos, _ = p.multiplyTransforms(cup_pos, cup_orient, self.cup_top_center_offset, [0, 0, 0, 1], physicsClientId=self.id)
        reward_distance = -np.linalg.norm(self.target_pos - np.array(cup_top_center_pos)) # Penalize distances between top of cup and mouth
        reward_action = -np.sum(np.square(action)) # Penalize actions
        # Encourage robot to have a tilted end effector / cup
        cup_euler = p.getEulerFromQuaternion(cup_orient, physicsClientId=self.id)
        reward_tilt = -abs(cup_euler[0] + np.pi/2) if self.robot_type == 'jaco' else -abs(cup_euler[0] - np.pi/2)

        reward = self.config('distance_weight')*reward_distance + self.config('action_weight')*reward_action + self.config('cup_tilt_weight')*reward_tilt + self.config('drinking_reward_weight')*reward_water + preferences_score

        if self.gui and reward_water != 0:
            print('Task success:', self.task_success, 'Water reward:', reward_water)

        info = {'total_force_on_human': total_force_on_human, 'task_success': int(self.task_success >= self.total_water_count*self.config('task_success_threshold')), 'action_robot_len': self.action_robot_len, 'action_human_len': self.action_human_len, 'obs_robot_len': self.obs_robot_len, 'obs_human_len': self.obs_human_len}
        done = False

        return obs, reward, done, info 

def getObservation(self):
        """
        Returns the position and angle of the effector
        :return: ([float])
        """
        observation = []
        state = p.getLinkState(self.kuka_uid, self.kuka_gripper_index)
        pos = state[0]
        orn = state[1]
        euler = p.getEulerFromQuaternion(orn)

        observation.extend(list(pos))
        observation.extend(list(euler))

        return observation 

def get_orientation_eulerian(self):
        return p.getEulerFromQuaternion(self.get_orientation()) 

def _moveToCallback(self, msg):
        """
        INTERNAL METHOD, callback triggered when a message is received on the
        '/move_base_simple/goal' topic. It allows to move the robot's base

        Parameters:
            msg - a ROS message containing a pose stamped with a speed, or a
            simple pose stamped (depending on which version of the naoqi_driver
            is used, the "official" one from ros-naoqi or the "non official"
            softbankrobotics-research fork). The type of the message is the
            following: geometry_msgs::PoseStamped for the "official",
            naoqi_bridge_msgs::PoseStampedWithSpeed for the "non-official".
            An alias is given to the message type: MovetoPose
        """

        if OFFICIAL_DRIVER:
            pose = msg.pose
            frame = 0
            frame_id = msg.header.frame_id
            speed = None
        else:
            pose = msg.pose_stamped.pose
            frame = msg.referenceFrame
            frame_id = msg.pose_stamped.header.frame_id
            speed = msg.speed_percentage *\
                PepperBaseController.MAX_LINEAR_VELOCITY +\
                PepperBaseController.MIN_LINEAR_VELOCITY

        try:
            assert frame not in [
                PepperVirtual.FRAME_ROBOT,
                PepperVirtual.FRAME_WORLD]

            if frame_id == "odom":
                frame = PepperVirtual.FRAME_WORLD
            elif frame_id == "base_footprint":
                frame = PepperVirtual.FRAME_ROBOT
            else:
                raise pybullet.error(
                    "Incorrect reference frame for move_base_simple, please "
                    "modify the content of your message")

        except AssertionError:
            pass

        x = pose.position.x
        y = pose.position.y
        theta = pybullet.getEulerFromQuaternion([
            pose.orientation.x,
            pose.orientation.y,
            pose.orientation.z,
            pose.orientation.w])[-1]

        self.robot.moveTo(
            x,
            y,
            theta,
            frame=frame,
            speed=speed,
            _async=True) 

def get_observation(self):
        # Create observation state
        observation = []
        observation_lim = []

        # Get state of the end-effector link
        state = p.getLinkState(self.robot_id, self.end_eff_idx, computeLinkVelocity=1,
                                computeForwardKinematics=1, physicsClientId=self._physics_client_id)

        # ------------------------- #
        # --- Cartesian 6D pose --- #
        # ------------------------- #
        pos = state[0]
        quat = state[1]

        observation.extend(list(pos))
        observation_lim.extend(list(self._workspace_lim))

        # cartesian orientation
        if self._control_eu_or_quat is 0:
            euler = p.getEulerFromQuaternion(quat)

            observation.extend(list(euler))
            observation_lim.extend(self._eu_lim)

        else:
            observation.extend(list(quat))
            observation_lim.extend([[-1, 1], [-1, 1], [-1, 1], [-1, 1]])

        # --------------------------------- #
        # --- Cartesian linear velocity --- #
        # --------------------------------- #
        vel_l = state[6]

        observation.extend(list(vel_l))
        observation_lim.extend([[-1, 1], [-1, 1], [-1, 1]])

        # ------------------- #
        # --- Joint poses --- #
        # ------------------- #
        joint_states = p.getJointStates(self.robot_id, self._joints_to_control, physicsClientId=self._physics_client_id)
        joint_poses = [x[0] for x in joint_states]

        observation.extend(list(joint_poses))
        observation_lim.extend([[self.ll[i], self.ul[i]] for i, idx in enumerate(self._joint_name_to_ids.values())
                                if idx in self._joints_to_control])

        return observation, observation_lim 

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 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)