def regulate_jnts(self):
"""
check if the given jntvalues is inside the oeprating range
The joint values out of range will be pulled back to their maxima
:return: Two parameters, one is true or false indicating if the joint values are inside the range or not
The other is the joint values after dragging.
If the joints were not dragged, the same joint values will be returned
author: weiwei
date: 20161205
"""
counter = 0
for id in self.jlc_object.tgtjnts:
if self.jlc_object.jnts[id]["type"] == 'revolute':
if self.jlc_object.jnts[id]['motion_rng'][
1] - self.jlc_object.jnts[id]['motion_rng'][
0] >= math.pi * 2:
rm.regulate_angle(
self.jlc_object.jnts[id]['motion_rng'][0],
self.jlc_object.jnts[id]['motion_rng'][1],
self.jlc_object.jnts[id]["movement"])
counter += 1
def regulate_jnts_pmpi(self):
"""
TODO allow settings for pmpi pm 2pi
the function move all joints back to -360,360
due to improper operations, some joints could be out of 360
this function moves the outlier joints back
:return:
author: weiwei
date: 20180202
"""
jnt_values = self.get_jnt_values()
regulated_jnt_values = rm.regulate_angle(-math.pi, math.pi, jnt_values)
self.move_jnts(regulated_jnt_values)