Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
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
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
#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
#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
#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