1. Advisor
  2. Klampt
  3. Klampt code examples
View all Klampt analysis

How to use Klampt - 10 common examples

To help you get started, we’ve selected a few Klampt examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

github krishauser / IROS2016ManipulationChallenge / plugins / reflex_col.py View on Github external
def apply_tendon_forces(self,i,link1,link2,rest_length):
        tendon_c2 = 30000.0
        tendon_c1 = 10000.0
        b0 = self.sim.body(self.model.robot.link(self.model.proximal_links[0]-3))
        b1 = self.sim.body(self.model.robot.link(link1))
        b2 = self.sim.body(self.model.robot.link(link2))
        p0w = se3.apply(b1.getTransform(),self.tendon0_local)
        p1w = se3.apply(b1.getTransform(),self.tendon1_local)
        p2w = se3.apply(b2.getTransform(),self.tendon2_local)

        d = vectorops.distance(p1w,p2w)
        if d > rest_length:
            #apply tendon force
            direction = vectorops.unit(vectorops.sub(p2w,p1w))
            f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
            #print d,rest_length
            #print "Force magnitude",self.model.robot.link(link1).getName(),":",f
            f = min(f,100)
            #tendon routing force
            straight = vectorops.unit(vectorops.sub(p2w,p0w))
            pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
            pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
            tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
github krishauser / IROS2016ManipulationChallenge / plugins / reflex_col.py View on Github external
def apply_tendon_forces(self,i,link1,link2,rest_length):
        tendon_c2 = 30000.0
        tendon_c1 = 10000.0
        b0 = self.sim.body(self.model.robot.link(self.model.proximal_links[0]-3))
        b1 = self.sim.body(self.model.robot.link(link1))
        b2 = self.sim.body(self.model.robot.link(link2))
        p0w = se3.apply(b1.getTransform(),self.tendon0_local)
        p1w = se3.apply(b1.getTransform(),self.tendon1_local)
        p2w = se3.apply(b2.getTransform(),self.tendon2_local)

        d = vectorops.distance(p1w,p2w)
        if d > rest_length:
            #apply tendon force
            direction = vectorops.unit(vectorops.sub(p2w,p1w))
            f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
            #print d,rest_length
            #print "Force magnitude",self.model.robot.link(link1).getName(),":",f
            f = min(f,100)
            #tendon routing force
            straight = vectorops.unit(vectorops.sub(p2w,p0w))
            pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
            pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
            tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
            cosangle = vectorops.dot(straight,tangential_axis)
            #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
github krishauser / IROS2016ManipulationChallenge / plugins / reflex_col.py View on Github external
f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
            #print d,rest_length
            #print "Force magnitude",self.model.robot.link(link1).getName(),":",f
            f = min(f,100)
            #tendon routing force
            straight = vectorops.unit(vectorops.sub(p2w,p0w))
            pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
            pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
            tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
            cosangle = vectorops.dot(straight,tangential_axis)
            #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
            base_direction = so3.apply(b0.getTransform()[0],[0,0,-1])
            b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04))
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local)
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local)
            b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local)
            self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f)
            self.forces[i][2] = vectorops.mul(direction,f)
        else:
            self.forces[i] = [None,None,None]
        return
github krishauser / IROS2016ManipulationChallenge / plugins / reflex_col.py View on Github external
direction = vectorops.unit(vectorops.sub(p2w,p1w))
            f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
            #print d,rest_length
            #print "Force magnitude",self.model.robot.link(link1).getName(),":",f
            f = min(f,100)
            #tendon routing force
            straight = vectorops.unit(vectorops.sub(p2w,p0w))
            pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
            pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
            tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
            cosangle = vectorops.dot(straight,tangential_axis)
            #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
            base_direction = so3.apply(b0.getTransform()[0],[0,0,-1])
            b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04))
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local)
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local)
            b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local)
            self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f)
            self.forces[i][2] = vectorops.mul(direction,f)
        else:
            self.forces[i] = [None,None,None]
        return
github krishauser / IROS2016ManipulationChallenge / plugins / reflex_col.py View on Github external
if d > rest_length:
            #apply tendon force
            direction = vectorops.unit(vectorops.sub(p2w,p1w))
            f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
            #print d,rest_length
            #print "Force magnitude",self.model.robot.link(link1).getName(),":",f
            f = min(f,100)
            #tendon routing force
            straight = vectorops.unit(vectorops.sub(p2w,p0w))
            pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
            pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
            tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
            cosangle = vectorops.dot(straight,tangential_axis)
            #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
            base_direction = so3.apply(b0.getTransform()[0],[0,0,-1])
            b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04))
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local)
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local)
            b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local)
            self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f)
            self.forces[i][2] = vectorops.mul(direction,f)
        else:
            self.forces[i] = [None,None,None]
        return
github krishauser / IROS2016ManipulationChallenge / plugins / reflex_col.py View on Github external
#apply tendon force
            direction = vectorops.unit(vectorops.sub(p2w,p1w))
            f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
            #print d,rest_length
            #print "Force magnitude",self.model.robot.link(link1).getName(),":",f
            f = min(f,100)
            #tendon routing force
            straight = vectorops.unit(vectorops.sub(p2w,p0w))
            pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
            pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
            tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
            cosangle = vectorops.dot(straight,tangential_axis)
            #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
            base_direction = so3.apply(b0.getTransform()[0],[0,0,-1])
            b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04))
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local)
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local)
            b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local)
            self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f)
            self.forces[i][2] = vectorops.mul(direction,f)
        else:
            self.forces[i] = [None,None,None]
        return
github krishauser / IROS2016ManipulationChallenge / plugins / reflex_col.py View on Github external
#print d,rest_length
            #print "Force magnitude",self.model.robot.link(link1).getName(),":",f
            f = min(f,100)
            #tendon routing force
            straight = vectorops.unit(vectorops.sub(p2w,p0w))
            pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
            pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
            tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
            cosangle = vectorops.dot(straight,tangential_axis)
            #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
            base_direction = so3.apply(b0.getTransform()[0],[0,0,-1])
            b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04))
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local)
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local)
            b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local)
            self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f)
            self.forces[i][2] = vectorops.mul(direction,f)
        else:
            self.forces[i] = [None,None,None]
        return
github krishauser / IROS2016ManipulationChallenge / plugins / reflex_col.py View on Github external
#print "Force magnitude",self.model.robot.link(link1).getName(),":",f
            f = min(f,100)
            #tendon routing force
            straight = vectorops.unit(vectorops.sub(p2w,p0w))
            pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
            pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
            tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
            cosangle = vectorops.dot(straight,tangential_axis)
            #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
            base_direction = so3.apply(b0.getTransform()[0],[0,0,-1])
            b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04))
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local)
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local)
            b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local)
            self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f)
            self.forces[i][2] = vectorops.mul(direction,f)
        else:
            self.forces[i] = [None,None,None]
        return
github krishauser / IROS2016ManipulationChallenge / plugins / reflex_col.py View on Github external
p0w = se3.apply(b1.getTransform(),self.tendon0_local)
        p1w = se3.apply(b1.getTransform(),self.tendon1_local)
        p2w = se3.apply(b2.getTransform(),self.tendon2_local)

        d = vectorops.distance(p1w,p2w)
        if d > rest_length:
            #apply tendon force
            direction = vectorops.unit(vectorops.sub(p2w,p1w))
            f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
            #print d,rest_length
            #print "Force magnitude",self.model.robot.link(link1).getName(),":",f
            f = min(f,100)
            #tendon routing force
            straight = vectorops.unit(vectorops.sub(p2w,p0w))
            pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
            pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
            tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
            cosangle = vectorops.dot(straight,tangential_axis)
            #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
            base_direction = so3.apply(b0.getTransform()[0],[0,0,-1])
            b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04))
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local)
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local)
            b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local)
            self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f)
            self.forces[i][2] = vectorops.mul(direction,f)
        else:
            self.forces[i] = [None,None,None]
        return
github krishauser / IROS2016ManipulationChallenge / plugins / reflex_col.py View on Github external
d = vectorops.distance(p1w,p2w)
        if d > rest_length:
            #apply tendon force
            direction = vectorops.unit(vectorops.sub(p2w,p1w))
            f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
            #print d,rest_length
            #print "Force magnitude",self.model.robot.link(link1).getName(),":",f
            f = min(f,100)
            #tendon routing force
            straight = vectorops.unit(vectorops.sub(p2w,p0w))
            pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
            pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
            tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
            cosangle = vectorops.dot(straight,tangential_axis)
            #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
            base_direction = so3.apply(b0.getTransform()[0],[0,0,-1])
            b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04))
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local)
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local)
            b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local)
            self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f)
            self.forces[i][2] = vectorops.mul(direction,f)
        else:
            self.forces[i] = [None,None,None]
        return

Klampt

Python API to Klamp't, a package for robot modeling, simulating, planning, optimization, and visualization

GitHub
BSD-3-Clause
Latest version published 3 months ago

Package Health Score

67 / 100
Full package analysis

Popular Klampt functions

Similar packages