Python pybullet.VELOCITY_CONTROL Examples

The following are 16 code examples of pybullet.VELOCITY_CONTROL(). You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example. You may also want to check out all available functions/classes of the module pybullet , or try the search function .
Example #1
Source File: cartpole_bullet.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up 
def _step(self, action):
    p.stepSimulation()
#    time.sleep(self.timeStep)
    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
    theta, theta_dot, x, x_dot = self.state

    dv = 0.1
    deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]

    p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3]))

    done =  x < -self.x_threshold \
                or x > self.x_threshold \
                or theta < -self.theta_threshold_radians \
                or theta > self.theta_threshold_radians
    reward = 1.0

    return np.array(self.state), reward, done, {}
Example #2
Source File: cartpole_bullet.py    From soccer-matlab with BSD 2-Clause "Simplified" License 6 votes vote down vote up 
def _reset(self):
#    print("-----------reset simulation---------------")
    p.resetSimulation()
    self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
    self.timeStep = 0.01
    p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
    p.setGravity(0,0, -10)
    p.setTimeStep(self.timeStep)
    p.setRealTimeSimulation(0)

    initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
    initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
    p.resetJointState(self.cartpole, 1, initialAngle)
    p.resetJointState(self.cartpole, 0, initialCartPos)

    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]

    return np.array(self.state)
Example #3
Source File: robot_bases.py    From soccer-matlab with BSD 2-Clause "Simplified" License 5 votes vote down vote up 
def set_velocity(self, velocity):
		self._p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,pybullet.VELOCITY_CONTROL, targetVelocity=velocity)
Example #4
Source File: simulation.py    From onshape-to-robot with MIT License 5 votes vote down vote up 
def setJoints(self, joints):
        """Set joint targets for motor control in simulation
        
        Arguments:
            joints {dict} -- dict of joint name -> angle (float, radian)
        
        Raises:
            Exception: if a joint is not found, exception is raised

        Returns:
            applied {dict} -- dict of joint states (position, velocity, reaction forces, applied torque)
        """
        applied = {}

        for name in joints.keys():
            if name in self.joints:
                if name.endswith('_speed'):
                    p.setJointMotorControl2(
                    self.robot, self.joints[name], p.VELOCITY_CONTROL, targetVelocity=joints[name])
                else:
                    if name in self.maxTorques:
                        maxTorque = self.maxTorques[name]
                        p.setJointMotorControl2(
                            self.robot, self.joints[name], p.POSITION_CONTROL, joints[name], force=maxTorque)
                    else:
                        p.setJointMotorControl2(
                            self.robot, self.joints[name], p.POSITION_CONTROL, joints[name])

                applied[name] = p.getJointState(self.robot, self.joints[name])
            else:
                raise Exception("Can't find joint %s" % name)

        return applied
Example #5
Source File: bullet_physics_engine.py    From NTP-vat-release with MIT License 5 votes vote down vote up 
def control_joint_vel(body, joint, vel, max_force=None):
        p.setJointMotorControl2(
                bodyIndex=body,
                jointIndex=joint,
                controlMode=p.VELOCITY_CONTROL,
                targetVelocity=vel,
                force=max_force)
Example #6
Source File: world_creation.py    From assistive-gym with MIT License 5 votes vote down vote up 
def setup_human_joints(self, human, joints_positions, controllable_joints, use_static_joints=True, human_reactive_force=None, human_reactive_gain=0.05):
        if self.human_impairment != 'tremor':
            self.human_tremors = np.zeros(len(controllable_joints))
        elif len(controllable_joints) == 4:
            self.human_tremors = self.np_random.uniform(np.deg2rad(-20), np.deg2rad(20), size=len(controllable_joints))
        else:
            self.human_tremors = self.np_random.uniform(np.deg2rad(-10), np.deg2rad(10), size=len(controllable_joints))
        # Set starting joint positions
        human_joint_states = p.getJointStates(human, jointIndices=list(range(p.getNumJoints(human, physicsClientId=self.id))), physicsClientId=self.id)
        human_joint_positions = np.array([x[0] for x in human_joint_states])
        for j in range(p.getNumJoints(human, physicsClientId=self.id)):
            set_position = None
            for j_index, j_angle in joints_positions:
                if j == j_index:
                    p.resetJointState(human, jointIndex=j, targetValue=j_angle, targetVelocity=0, physicsClientId=self.id)
                    set_position = j_angle
                    break
            if use_static_joints and j not in controllable_joints:
                # Make all non controllable joints on the person static by setting mass of each link (joint) to 0
                p.changeDynamics(human, j, mass=0, physicsClientId=self.id)
                # Set velocities to 0
                p.resetJointState(human, jointIndex=j, targetValue=human_joint_positions[j] if set_position is None else set_position, targetVelocity=0, physicsClientId=self.id)

        # By default, all joints have motors enabled by default that prevent free motion. Disable these motors in human
        for j in range(p.getNumJoints(human, physicsClientId=self.id)):
            p.setJointMotorControl2(human, jointIndex=j, controlMode=p.VELOCITY_CONTROL, force=0, physicsClientId=self.id)

        self.enforce_joint_limits(human)

        if human_reactive_force is not None:
            # NOTE: This runs a Position / Velocity PD controller for each joint motor on the human
            human_joint_states = p.getJointStates(human, jointIndices=controllable_joints, physicsClientId=self.id)
            target_human_joint_positions = np.array([x[0] for x in human_joint_states])
            forces = [human_reactive_force * self.human_strength] * len(target_human_joint_positions)
            p.setJointMotorControlArray(human, jointIndices=controllable_joints, controlMode=p.POSITION_CONTROL, targetPositions=target_human_joint_positions, positionGains=np.array([human_reactive_gain]*len(forces)), forces=forces, physicsClientId=self.id)
Example #7
Source File: turtlebot.py    From costar_plan with Apache License 2.0 5 votes vote down vote up 
def base(self, cmd):
        maxForce = 500
        #for j in range (0,i):
        
        vr = cmd[0];
        va = cmd[1];
        
        wheel_sep_ = 0.34; # TODO: Verify

        wheel_speed_left = vr - va * (wheel_sep_) / 2;
        wheel_speed_right = vr + va * (wheel_sep_) / 2;
        pb.setJointMotorControl2(self.handle, jointIndex=self.left_wheel_index, controlMode=pb.VELOCITY_CONTROL,targetVelocity = wheel_speed_left,force = maxForce)
        pb.setJointMotorControl2(self.handle, jointIndex=self.right_wheel_index, controlMode=pb.VELOCITY_CONTROL,targetVelocity = wheel_speed_right,force = maxForce)
Example #8
Source File: robot_bases.py    From GtS with MIT License 5 votes vote down vote up 
def set_velocity(self, velocity):
        """Set velocity of joint
           Velocity is defined in real world scale """
        if self.jointType == p.JOINT_PRISMATIC:
            velocity = np.array(velocity) / self.scale
        p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.VELOCITY_CONTROL, targetVelocity=velocity) # , positionGain=0.1, velocityGain=0.1)
Example #9
Source File: robot_bases.py    From midlevel-reps with MIT License 5 votes vote down vote up 
def set_velocity(self, velocity):
        """Set velocity of joint
           Velocity is defined in real world scale """
        if self.jointType == p.JOINT_PRISMATIC:
            velocity = np.array(velocity) / self.scale
        p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.VELOCITY_CONTROL, targetVelocity=velocity) # , positionGain=0.1, velocityGain=0.1)
Example #10
Source File: gym_mujoco_xml_env.py    From bullet-gym with MIT License 5 votes vote down vote up 
def set_velocity(self, velocity):
		p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.VELOCITY_CONTROL, targetVelocity=velocity)
Example #11
Source File: gym_mujoco_xml_env.py    From bullet-gym with MIT License 5 votes vote down vote up 
def disable_motor(self):
		p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=p.VELOCITY_CONTROL, force=0)
Example #12
Source File: MJCFCommon.py    From bullet-gym with MIT License 5 votes vote down vote up 
def set_velocity(self, velocity):
        p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.VELOCITY_CONTROL, targetVelocity=velocity)
Example #13
Source File: MJCFCommon.py    From bullet-gym with MIT License 5 votes vote down vote up 
def disable_motor(self):
        p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=p.VELOCITY_CONTROL, force=0)
Example #14
Source File: balancebot_env.py    From balance-bot with MIT License 5 votes vote down vote up 
def _assign_throttle(self, action):
        dv = 0.1
        deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]
        vt = clamp(self.vt + deltav, -self.maxV, self.maxV)
        self.vt = vt

        p.setJointMotorControl2(bodyUniqueId=self.botId, 
                                jointIndex=0, 
                                controlMode=p.VELOCITY_CONTROL, 
                                targetVelocity=vt)
        p.setJointMotorControl2(bodyUniqueId=self.botId, 
                                jointIndex=1, 
                                controlMode=p.VELOCITY_CONTROL, 
                                targetVelocity=-vt)
Example #15
Source File: icub_env_with_hands.py    From pybullet-robot-envs with GNU Lesser General Public License v2.1 5 votes vote down vote up 
def grasp(self, pos=None):
        # close fingers

        if self._control_arm is 'l':
            idx_thumb = self._joint_name_to_ids['l_hand::l_tj2']
            idx_fingers = [self._joint_name_to_ids[jn] for jn in self.joint_groups['l_hand']]
        else:
            idx_thumb = self._joint_name_to_ids['r_hand::r_tj2']
            idx_fingers = [self._joint_name_to_ids[jn] for jn in self.joint_groups['r_hand']]

        # # set also position to other joints to avoid weird movements
        # not_idx_fingers = [idx for idx in self._joints_to_control if idx not in idx_fingers]
        #
        # joint_states = p.getJointStates(self.robot_id, not_idx_fingers)
        # joint_poses = [x[0] for x in joint_states]
        # p.setJointMotorControlArray(self.robot_id, not_idx_fingers, p.POSITION_CONTROL,
        #                             targetPositions=joint_poses,
        #                             positionGains=[0.1] * len(not_idx_fingers),
        #                             velocityGains=[1.0] * len(not_idx_fingers),
        #                             physicsClientId=self._physics_client_id)

        position_control = True
        if position_control:
            if pos is None:
                pos = self._grasp_pos
            p.setJointMotorControlArray(self.robot_id, idx_fingers, p.POSITION_CONTROL,
                                        targetPositions=pos,
                                        positionGains=[0.1] * len(idx_fingers),
                                        velocityGains=[1.0] * len(idx_fingers),
                                        forces=[10] * len(idx_fingers),
                                        physicsClientId=self._physics_client_id)

        else:
            # vel = [0, 1, 1, 1.2, 0, 1, 1, 1.2, 0, 1, 1, 1.2, 0, 1, 1, 1.2, 1.57, 1.5, 1.1, 1.1]
            vel = [0, 0.5, 0.6, 0.6, 0, 0.5, 0.6, 0.6, 0, 0.5, 0.6, 0.6, 0, 0.5, 0.6, 0.6, 0, 0.5, 0.6, 0.6]

            p.setJointMotorControlArray(self.robot_id, idx_fingers, p.VELOCITY_CONTROL,
                                        targetVelocities=vel,
                                        positionGains=[0.1] * len(idx_fingers),
                                        velocityGains=[1.0] * len(idx_fingers),
                                        physicsClientId=self._physics_client_id)
Example #16
Source File: minitaur.py    From soccer-matlab with BSD 2-Clause "Simplified" License 4 votes vote down vote up 
def resetPose(self):
    kneeFrictionForce = 0
    halfpi = 1.57079632679
    kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)

    #left front leg
    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle)
    p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
    self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi)
    self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)

    #left back leg
    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle)
    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle)
    p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
    self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi)
    self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
 
    #right front leg
    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle)
    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle)
    p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
    self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi)
    self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
 

    #right back leg
    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle)
    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle)
    p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
    self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi)
    self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)