Python pybullet.getJointInfo() Examples

def setRobot(self, robot, physicsClientId=None):
        self._robot = robot
        if physicsClientId != None:
            self._physicsClientId = physicsClientId

        joint_id_list = []
        joint_pos_list = []
        self._joint_number = 0
        for i in range(p.getNumJoints(robot)):
            jointInfo = p.getJointInfo(robot, i)
            if jointInfo[2] == 0:
                joint_id_list.append(jointInfo[0])
                joint_pos_list.append(p.getJointState(robot, jointInfo[0])[0])
                self._joint_number += 1
        print(self._joint_number)
        self._joint_id = np.array(joint_id_list, dtype=np.int32)
        self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
        self._joint_currentPos = np.array(joint_pos_list, dtype=np.float) 

def get_joint_ranges(self):
        lower_limits, upper_limits, joint_ranges, rest_poses = [], [], [], []

        for joint_name in self._joint_name_to_ids.keys():
            jointInfo = p.getJointInfo(self.robot_id, self._joint_name_to_ids[joint_name], physicsClientId=self._physics_client_id)

            ll, ul = jointInfo[8:10]
            jr = ul - ll
            # For simplicity, assume resting state == initial state
            rp = self.initial_positions[joint_name]
            lower_limits.append(ll)
            upper_limits.append(ul)
            joint_ranges.append(jr)
            rest_poses.append(rp)

        return lower_limits, upper_limits, joint_ranges, rest_poses 

def get_joint_ranges(self):
        lower_limits, upper_limits, joint_ranges, rest_poses, joint_dumping = [], [], [], [], []

        for joint_name in self._joint_name_to_ids.keys():
            jointInfo = p.getJointInfo(self.robot_id, self._joint_name_to_ids[joint_name], physicsClientId=self._physics_client_id)

            ll, ul = jointInfo[8:10]
            jr = ul - ll
            # For simplicity, assume resting state == initial state
            rp = self.initial_positions[joint_name]
            lower_limits.append(ll)
            upper_limits.append(ul)
            joint_ranges.append(jr)
            rest_poses.append(rp)
            joint_dumping.append(0.1 if self._joint_name_to_ids[joint_name] in self._joints_to_control else 100.)

        return lower_limits, upper_limits, joint_ranges, rest_poses, joint_dumping 

def enforce_hard_human_joint_limits(self):
        if not self.human_controllable_joint_indices:
            return
        # Enforce joint limits. Sometimes, external forces and break the person's hard joint limits.
        joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id)
        joint_positions = np.array([x[0] for x in joint_states])
        if self.human_joint_lower_limits is None:
            self.human_joint_lower_limits = []
            self.human_joint_upper_limits = []
            for i, j in enumerate(self.human_controllable_joint_indices):
                joint_info = p.getJointInfo(self.human, j, physicsClientId=self.id)
                joint_name = joint_info[1]
                joint_pos = joint_positions[i]
                lower_limit = joint_info[8]
                upper_limit = joint_info[9]
                self.human_joint_lower_limits.append(lower_limit)
                self.human_joint_upper_limits.append(upper_limit)
                # print(joint_name, joint_pos, lower_limit, upper_limit)
        for i, j in enumerate(self.human_controllable_joint_indices):
            if joint_positions[i] < self.human_joint_lower_limits[i]:
                p.resetJointState(self.human, jointIndex=j, targetValue=self.human_joint_lower_limits[i], targetVelocity=0, physicsClientId=self.id)
            elif joint_positions[i] > self.human_joint_upper_limits[i]:
                p.resetJointState(self.human, jointIndex=j, targetValue=self.human_joint_upper_limits[i], targetVelocity=0, physicsClientId=self.id) 

def enforce_joint_limits(self, body):
        # Enforce joint limits
        joint_states = p.getJointStates(body, jointIndices=list(range(p.getNumJoints(body, physicsClientId=self.id))), physicsClientId=self.id)
        joint_positions = np.array([x[0] for x in joint_states])
        lower_limits = []
        upper_limits = []
        for j in range(p.getNumJoints(body, physicsClientId=self.id)):
            joint_info = p.getJointInfo(body, j, physicsClientId=self.id)
            joint_name = joint_info[1]
            joint_pos = joint_positions[j]
            lower_limit = joint_info[8]
            upper_limit = joint_info[9]
            if lower_limit == 0 and upper_limit == -1:
                lower_limit = -1e10
                upper_limit = 1e10
            lower_limits.append(lower_limit)
            upper_limits.append(upper_limit)
            # print(joint_name, joint_pos, lower_limit, upper_limit)
            if joint_pos < lower_limit:
                p.resetJointState(body, jointIndex=j, targetValue=lower_limit, targetVelocity=0, physicsClientId=self.id)
            elif joint_pos > upper_limit:
                p.resetJointState(body, jointIndex=j, targetValue=upper_limit, targetVelocity=0, physicsClientId=self.id)
        lower_limits = np.array(lower_limits)
        upper_limits = np.array(upper_limits)
        return lower_limits, upper_limits 

def contactPoints(self):
        """Gets all contact points and forces
        
        Returns:
            list -- list of entries (link_name, position in m, force in N)
        """
        result = []
        contacts = p.getContactPoints(bodyA=self.floor, bodyB=self.robot)
        for contact in contacts:
            link_index = contact[4]
            if link_index >= 0:
                link_name = (p.getJointInfo(self.robot, link_index)[12]).decode()
            else:
                link_name = 'base'
            result.append((link_name, contact[6], contact[9]))

        return result 

def reset(self):
        """
        Reset the environment
        """
        objects = p.loadSDF(os.path.join(self.urdf_root_path, "kuka_iiwa/kuka_with_gripper2.sdf"))
        self.kuka_uid = objects[0]

        p.resetBasePositionAndOrientation(self.kuka_uid, [-0.100000, 0.000000, -0.15],
                                          [0.000000, 0.000000, 0.000000, 1.000000])
        self.joint_positions = [0.006418, 0.113184, -0.011401, -1.289317, 0.005379, 1.737684, -0.006539, 0.000048,
                                -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200]
        self.num_joints = p.getNumJoints(self.kuka_uid)
        for jointIndex in range(self.num_joints):
            p.resetJointState(self.kuka_uid, jointIndex, self.joint_positions[jointIndex])
            p.setJointMotorControl2(self.kuka_uid, jointIndex, p.POSITION_CONTROL,
                                    targetPosition=self.joint_positions[jointIndex], force=self.max_force)

        self.end_effector_pos = np.array([0.537, 0.0, 0.5])
        self.end_effector_angle = 0

        self.motor_names = []
        self.motor_indices = []

        for i in range(self.num_joints):
            joint_info = p.getJointInfo(self.kuka_uid, i)
            q_index = joint_info[3]
            if q_index > -1:
                self.motor_names.append(str(joint_info[1]))
                self.motor_indices.append(i) 

def findGraspFrame(self):
        '''
        Helper function to look up the grasp frame associated with a robot.
        '''
        joints = pb.getNumJoints(self.handle)
        grasp_idx = None
        for i in xrange(joints):
            idx, name, jtype, qidx, \
                uidx, flags, damping, friction, \
                lower, upper, max_force, max_vel, \
                link_name = pb.getJointInfo(self.handle, i)
            #info = pb.getJointInfo(self.handle, i)
            #print(info)
            #link_name = info[12]
            assert(isinstance(link_name, str))
            if link_name == self.grasp_link:
                grasp_idx = i
                break
        return grasp_idx 

def __init__(self, joint_name, bodies, bodyIndex, jointIndex, scale, model_type):
        self.bodies = bodies
        self.bodyIndex = bodyIndex
        self.jointIndex = jointIndex
        self.joint_name = joint_name
        _,_,self.jointType,_,_,_,_,_,self.lowerLimit, self.upperLimit,_,_,_, _,_,_,_ = p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex)
        self.power_coeff = 0
        if model_type=="MJCF":
            self.scale = scale
        else:
            self.scale = 1
        if self.jointType == p.JOINT_PRISMATIC:
            self.upperLimit *= self.scale
            self.lowerLimit *= self.scale 

def _BuildJointNameToIdDict(self):
        num_joints = p.getNumJoints(self.minitaur)
        self.joint_name_to_id = {}
        for i in range(num_joints):
            joint_info = p.getJointInfo(self.minitaur, i)
            self.joint_name_to_id[joint_info[1].decode("UTF-8")] = joint_info[0] 

def __init__(self, joint_name, bodies, bodyIndex, jointIndex, scale, model_type):
        self.bodies = bodies
        self.bodyIndex = bodyIndex
        self.jointIndex = jointIndex
        self.joint_name = joint_name
        _,_,self.jointType,_,_,_,_,_,self.lowerLimit, self.upperLimit,_,_,_, _,_,_,_ = p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex)
        self.power_coeff = 0
        if model_type=="MJCF":
            self.scale = scale
        else:
            self.scale = 1
        if self.jointType == p.JOINT_PRISMATIC:
            self.upperLimit *= self.scale
            self.lowerLimit *= self.scale 

def __init__(self, joint_name, bodies, bodyIndex, jointIndex):
		self.bodies = bodies
		self.bodyIndex = bodyIndex
		self.jointIndex = jointIndex
		self.joint_name = joint_name
		_,_,_,_,_,_,_,_,self.lowerLimit, self.upperLimit,_,_,_ = p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex)
		self.power_coeff = 0 

def getScene(self, bodies):
        parts = {}
        joints = {}
        for i in range(len(bodies)):
            for j in range(p.getNumJoints(bodies[i])):
                _,joint_name,_,_,_,_,_,_,_,_,_,_,part_name = p.getJointInfo(bodies[i],j)
                joints[joint_name] = Joint(bodies,i,j)
                joints[joint_name].disable_motor()
                
                parts[part_name] = BodyPart(bodies,i,j)
        return parts, joints 

def __init__(self, bodies, bodyIndex, jointIndex):
        self.bodies = bodies
        self.bodyIndex = bodyIndex
        self.jointIndex = jointIndex
        _,_,_,_,_,_,_,_,self.lowerLimit, self.upperLimit,_,_,_ = p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex) 

def __init__(self, robot, physicsClientId, timeStep, kp, kd, torque, max_velocity):
        self._robot = robot
        self._physicsClientId = physicsClientId
        jointNameToId = {}
        joint_id_list = []
        joint_pos_list = []
        self._joint_number = 0
        for i in range(p.getNumJoints(self._robot, physicsClientId=self._physicsClientId)):
            jointInfo = p.getJointInfo(self._robot, i, physicsClientId=self._physicsClientId)
            if jointInfo[2] == 0:
                joint_id_list.append(jointInfo[0])
                joint_pos_list.append(p.getJointState(self._robot, jointInfo[0], physicsClientId=self._physicsClientId)[0])
                jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
                self._joint_number += 1
        self._joint_id = np.array(joint_id_list, dtype=np.int32)
        self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
        self._joint_currentPos = np.array(joint_pos_list, dtype=np.float)

        self._jointNameToId = jointNameToId
        self._kp = kp
        self._kd = kd
        self._torque = torque
        self._max_velocity = max_velocity
        self._timeStep = timeStep
        # print(self._joint_id)
        # print(self._joint_targetPos)
        # print(self._jointNameToId) 

def setRobot(self, robot, physicsClientId=None):
        self._robot = robot
        if physicsClientId != None:
            self._physicsClientId = physicsClientId
        jointNameToId = {}
        joint_id_list = []
        joint_pos_list = []
        self._joint_number = 0
        for i in range(p.getNumJoints(self._robot, physicsClientId=self._physicsClientId)):
            jointInfo = p.getJointInfo(self._robot, i, physicsClientId=self._physicsClientId)
            if jointInfo[2] == 0:
                joint_id_list.append(jointInfo[0])
                joint_pos_list.append(p.getJointState(self._robot, jointInfo[0], physicsClientId=self._physicsClientId)[0])
                jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
                self._joint_number += 1
        self._joint_id = np.array(joint_id_list, dtype=np.int32)
        self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
        self._joint_currentPos = np.array(joint_pos_list, dtype=np.float) 

def __init__(self, robot, physicsClientId, timeStep, kp, kd, torque, max_velocity):
        self._robot = robot
        self._physicsClientId = physicsClientId
        jointNameToId = {}
        joint_id_list = []
        joint_pos_list = []
        self._joint_number = 0
        for i in range(p.getNumJoints(self._robot, physicsClientId=self._physicsClientId)):
            jointInfo = p.getJointInfo(self._robot, i, physicsClientId=self._physicsClientId)
            if jointInfo[2] == 0:
                joint_id_list.append(jointInfo[0])
                joint_pos_list.append(p.getJointState(
                    self._robot, jointInfo[0], physicsClientId=self._physicsClientId)[0])
                jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
                self._joint_number += 1
        self._joint_id = np.array(joint_id_list, dtype=np.int32)
        self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
        self._joint_currentPos = np.array(joint_pos_list, dtype=np.float)

        self._jointNameToId = jointNameToId
        self._kp = kp
        self._kd = kd
        self._torque = torque
        self._max_velocity = max_velocity
        self._timeStep = timeStep
        # print(self._joint_id)
        # print(self._joint_targetPos)
        # print(self._jointNameToId) 

def reset(self):
        # Load robot model
        flags = p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES | p.URDF_USE_INERTIA_FROM_FILE
        self.robot_id = p.loadURDF(os.path.join(franka_panda.get_data_path(), "panda_model.urdf"),
                                   basePosition=self._base_position, useFixedBase=True, flags=flags,
                                   physicsClientId=self._physics_client_id)

        assert self.robot_id is not None, "Failed to load the panda model"

        # reset joints to home position
        num_joints = p.getNumJoints(self.robot_id, physicsClientId=self._physics_client_id)
        idx = 0
        for i in range(num_joints):
            joint_info = p.getJointInfo(self.robot_id, i, physicsClientId=self._physics_client_id)
            joint_name = joint_info[1].decode("UTF-8")
            joint_type = joint_info[2]

            if joint_type is p.JOINT_REVOLUTE or joint_type is p.JOINT_PRISMATIC:
                assert joint_name in self.initial_positions.keys()

                self._joint_name_to_ids[joint_name] = i

                p.resetJointState(self.robot_id, i, self.initial_positions[joint_name], physicsClientId=self._physics_client_id)
                p.setJointMotorControl2(self.robot_id, i, p.POSITION_CONTROL,
                                        targetPosition=self.initial_positions[joint_name],
                                        positionGain=0.2, velocityGain=1.0,
                                        physicsClientId=self._physics_client_id)

                idx += 1

        self.ll, self.ul, self.jr, self.rs = self.get_joint_ranges()

        if self._use_IK:

            self._home_hand_pose = [0.2, 0.0, 0.8,
                                    min(m.pi, max(-m.pi, m.pi)),
                                    min(m.pi, max(-m.pi, 0)),
                                    min(m.pi, max(-m.pi, 0))]

            self.apply_action(self._home_hand_pose)
            p.stepSimulation(physicsClientId=self._physics_client_id) 

def get_link_name(body, link):
        # Links and Joints have the corresponding UIDs
        _, joint_name, joint_type, _, _, _, damping, friction, \
                lower, upper, max_force, max_vel, _ = \
                p.getJointInfo(body, link)
        return joint_name # TODO 

def getMotorJointStates(robot):
	joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
	joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
	joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1]
	joint_positions = [state[0] for state in joint_states]
	joint_velocities = [state[1] for state in joint_states]
	joint_torques = [state[3] for state in joint_states]
	return joint_positions, joint_velocities, joint_torques 

def multiplyJacobian(robot, jacobian, vector):
	result = [0.0, 0.0, 0.0]
	i = 0
	for c in range(len(vector)):
		if p.getJointInfo(robot, c)[3] > -1:
			for r in range(3):
				result[r] += jacobian[r][i] * vector[c]
			i += 1
	return result 

def Step(stepIndex):
	for objectId in range(objectNum):
		record = log[stepIndex*objectNum+objectId]
		Id = record[2]
		pos = [record[3],record[4],record[5]]
		orn = [record[6],record[7],record[8],record[9]]
		p.resetBasePositionAndOrientation(Id,pos,orn)
		numJoints = p.getNumJoints(Id)
		for i in range (numJoints):
			jointInfo = p.getJointInfo(Id,i)
			qIndex = jointInfo[3]
			if qIndex > -1:
				p.resetJointState(Id,i,record[qIndex-7+17]) 

def getMotorJointStates(self, robot):
        import pybullet as p
        joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
        joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
        joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1]
        joint_positions = [state[0] for state in joint_states]
        joint_velocities = [state[1] for state in joint_states]
        joint_torques = [state[3] for state in joint_states]
        return joint_positions, joint_velocities, joint_torques 

def __init__(self):

        # initialize robot
        self.body = p.loadURDF("../soccer_description/models/soccerbot/model.xacro")
        self.state = RobotState()
        self.imu = -1
        self.imuMeasurements = JointMeasurement()
        self.joints = []
        self.motors = []

        # create a list of joints and find the IMU
        for i in range(p.getNumJoints(self.body)):
            self.joints.append(i)
            jointInfo = p.getJointInfo(self.body, i)
            if jointInfo[1].decode('ascii') == "torso_imu":
                self.imu = jointInfo[0]

        if self.imu == -1:
            raise AttributeError("Could not find robot's imu sensor from joint list")

        # rearrange joint order to match the order of positions found in the csv files. See:
        # https://docs.google.com/spreadsheets/d/1KgIYwm3fNen8yjLEa-FEWq-GnRUnBjyg4z64nZ2uBv8/edit#gid=775054312
        self.motors = self.joints[2:14] + self.joints[17:21] + self.joints[14:16] + self.joints[0:2]

        # initialize animations
        self.getupBackAnimation = Animation("./trajectories/getupback.csv")
        self.getupFrontAnimation = Animation("./trajectories/getupfront.csv")
        self.readyAnimation = Animation("./trajectories/ready.csv")
        self.standingAnimation = Animation("./trajectories/standing.csv")
        self.activeAnimation = None 

def setup_inverse_kinematics(self, urdf_path):
        """
        This function is responsible for doing any setup for inverse kinematics.
        Inverse Kinematics maps end effector (EEF) poses to joint angles that
        are necessary to achieve those poses. 
        """

        # These indices come from the urdf file we're using
        self.effector_right = 27
        self.effector_left = 45

        # Use PyBullet to handle inverse kinematics.
        # Set up a connection to the PyBullet simulator.
        p.connect(p.DIRECT)
        p.resetSimulation()

        self.ik_robot = p.loadURDF(urdf_path, (0, 0, 0), useFixedBase=1)

        # Relevant joints we care about. Many of the joints are fixed and don't count, so
        # we need this second map to use the right ones.
        self.actual = [13, 14, 15, 16, 17, 19, 20, 31, 32, 33, 34, 35, 37, 38]

        self.num_joints = p.getNumJoints(self.ik_robot)
        n = p.getNumJoints(self.ik_robot)
        self.rest = []
        self.lower = []
        self.upper = []
        self.ranges = []
        for i in range(n):
            info = p.getJointInfo(self.ik_robot, i)
            # Retrieve lower and upper ranges for each relevant joint
            if info[3] > -1:
                self.rest.append(p.getJointState(self.ik_robot, i)[0])
                self.lower.append(info[8])
                self.upper.append(info[9])
                self.ranges.append(info[9] - info[8])

        # Simulation will update as fast as it can in real time, instead of waiting for
        # step commands like in the non-realtime case.
        p.setRealTimeSimulation(1) 

def get_motor_joint_states(self, robot):
        num_joints = p.getNumJoints(robot, physicsClientId=self.id)
        joint_states = p.getJointStates(robot, range(num_joints), physicsClientId=self.id)
        joint_infos = [p.getJointInfo(robot, i, physicsClientId=self.id) for i in range(num_joints)]
        joint_states = [j for j, i in zip(joint_states, joint_infos) if i[2] != p.JOINT_FIXED]
        joint_positions = [state[0] for state in joint_states]
        joint_velocities = [state[1] for state in joint_states]
        joint_torques = [state[3] for state in joint_states]
        return joint_positions, joint_velocities, joint_torques 

def get_joint_name(body, joint):
        # Links and Joints have the corresponding UIDs
        _, joint_name, joint_type, _, _, _, damping, friction, \
                lower, upper, max_force, max_vel, _ = \
                p.getJointInfo(body, joint)
        return joint_name 

def get_joint_dynamics(body, joint):
        # Links and Joints have the corresponding UIDs
        _, joint_name, joint_type, _, _, _, damping, friction, \
                lower, upper, max_force, max_vel, _ = \
                p.getJointInfo(body, joint)
        dynamics = {
                'damping': damping,
                'friction': friction
                }
        return dynamics 

def get_joint_limit(body, joint):
        # Links and Joints have the corresponding UIDs
        _, joint_name, joint_type, _, _, _, damping, friction, \
                lower, upper, max_force, max_vel, _ = \
                p.getJointInfo(body, joint)
        limit = {
                'lower': lower,
                'upper': upper,
                'effort': max_force,
                'velocity': max_vel
                }
        return limit 

def ik(self, body, target_joint, target_pos, target_orient, ik_indices=range(29, 29+7), max_iterations=1000, half_range=False):
        key = '%d_%d' % (body, target_joint)
        if key not in self.ik_lower_limits:
            self.ik_lower_limits[key] = []
            self.ik_upper_limits[key] = []
            self.ik_joint_ranges[key] = []
            self.ik_rest_poses[key] = []
            j_names = []
            for j in range(p.getNumJoints(body, physicsClientId=self.id)):
                if p.getJointInfo(body, j, physicsClientId=self.id)[2] != p.JOINT_FIXED:
                    joint_info = p.getJointInfo(body, j, physicsClientId=self.id)
                    lower_limit = joint_info[8]
                    upper_limit = joint_info[9]
                    # print(len(self.ik_lower_limits[key]), joint_info[1], lower_limit, upper_limit)
                    if lower_limit == 0 and upper_limit == -1:
                        # lower_limit = -1e10
                        # upper_limit = 1e10
                        lower_limit = -2*np.pi
                        upper_limit = 2*np.pi
                    self.ik_lower_limits[key].append(lower_limit)
                    self.ik_upper_limits[key].append(upper_limit)
                    if not half_range:
                        self.ik_joint_ranges[key].append(upper_limit - lower_limit)
                    else:
                        self.ik_joint_ranges[key].append((upper_limit - lower_limit)/2.0)
                    # self.ik_rest_poses[key].append((upper_limit + lower_limit)/2.0)
                    j_names.append([len(j_names)] + list(joint_info[:2]))
        self.ik_rest_poses[key] = self.np_random.uniform(self.ik_lower_limits[key], self.ik_upper_limits[key]).tolist()
        if target_orient is not None:
            ik_joint_poses = np.array(p.calculateInverseKinematics(body, target_joint, targetPosition=target_pos, targetOrientation=target_orient, lowerLimits=self.ik_lower_limits[key], upperLimits=self.ik_upper_limits[key], jointRanges=self.ik_joint_ranges[key], restPoses=self.ik_rest_poses[key], maxNumIterations=max_iterations, physicsClientId=self.id))
        else:
            ik_joint_poses = np.array(p.calculateInverseKinematics(body, target_joint, targetPosition=target_pos, lowerLimits=self.ik_lower_limits[key], upperLimits=self.ik_upper_limits[key], jointRanges=self.ik_joint_ranges[key], restPoses=self.ik_rest_poses[key], maxNumIterations=max_iterations, physicsClientId=self.id))
        # print(j_names)
        # print(ik_joint_poses)
        # exit()
        target_joint_positions = ik_joint_poses[ik_indices]
        return target_joint_positions