<commit_before>package org.myrobotlab.service;

import java.util.HashMap;
import java.util.Map;

import org.myrobotlab.framework.Service;
import org.myrobotlab.kinematics.DHLink;
import org.myrobotlab.kinematics.DHRobotArm;
import org.myrobotlab.kinematics.Point;
import org.myrobotlab.logging.Level;
import org.myrobotlab.logging.LoggerFactory;
import org.myrobotlab.logging.LoggingFactory;
import org.myrobotlab.math.MathUtils;
import org.myrobotlab.service.interfaces.IKJointAnglePublisher;
import org.slf4j.Logger;

/**
 * 
 * InverseKinematics3D - This class provides a 3D based inverse kinematics implementation
 * that allows you to specify the robot arm geometry based on DH Parameters.
 * This will use a pseudo-inverse jacobian gradient descent approach to 
 * move the end affector to the desired x,y,z postions in space with 
 * respect to the base frame.
 * 
 * Rotation and Orientation information is not currently supported. (but should be easy to add)
 *
 * @author kwatters
 * 
 */
public class InverseKinematics3D extends Service implements IKJointAnglePublisher {

	private static final long serialVersionUID = 1L;
	public final static Logger log = LoggerFactory.getLogger(InverseKinematics3D.class.getCanonicalName());

	private DHRobotArm currentArm = null;
	
	
	
	public InverseKinematics3D(String n) {
		super(n);
		// TODO: init
	}

	public Point currentPosition() {
		return currentArm.getPalmPosition();		
	}
	
	public void moveTo(double x, double y, double z) {
		moveTo(new Point(x,y,z));
	}
	
	public void moveTo(Point p) {
		currentArm.moveToGoal(p);
		HashMap<String, Float> angleMap = new HashMap<String, Float>();
		for (DHLink l : currentArm.getLinks()) {
			String jointName = l.getName();
			double theta = l.getTheta();
			angleMap.put(jointName, (float)MathUtils.radToDeg(theta));<commit_msg>make sure the published angles are between 0 - 360 for ik3d service<commit_after>package org.myrobotlab.service;

import java.util.HashMap;
import java.util.Map;

import org.myrobotlab.framework.Service;
import org.myrobotlab.kinematics.DHLink;
import org.myrobotlab.kinematics.DHRobotArm;
import org.myrobotlab.kinematics.Point;
import org.myrobotlab.logging.Level;
import org.myrobotlab.logging.LoggerFactory;
import org.myrobotlab.logging.LoggingFactory;
import org.myrobotlab.math.MathUtils;
import org.myrobotlab.service.interfaces.IKJointAnglePublisher;
import org.slf4j.Logger;

/**
 * 
 * InverseKinematics3D - This class provides a 3D based inverse kinematics implementation
 * that allows you to specify the robot arm geometry based on DH Parameters.
 * This will use a pseudo-inverse jacobian gradient descent approach to 
 * move the end affector to the desired x,y,z postions in space with 
 * respect to the base frame.
 * 
 * Rotation and Orientation information is not currently supported. (but should be easy to add)
 *
 * @author kwatters
 * 
 */
public class InverseKinematics3D extends Service implements IKJointAnglePublisher {

	private static final long serialVersionUID = 1L;
	public final static Logger log = LoggerFactory.getLogger(InverseKinematics3D.class.getCanonicalName());

	private DHRobotArm currentArm = null;
	
	
	
	public InverseKinematics3D(String n) {
		super(n);
		// TODO: init
	}

	public Point currentPosition() {
		return currentArm.getPalmPosition();		
	}
	
	public void moveTo(double x, double y, double z) {
		moveTo(new Point(x,y,z));
	}
	
	public void moveTo(Point p) {
		currentArm.moveToGoal(p);
		HashMap<String, Float> angleMap = new HashMap<String, Float>();
		for (DHLink l : currentArm.getLinks()) {
			String jointName = l.getName();
			double theta = l.getTheta();
			// angles between 0 - 360 degrees..  not sure what people will really want?
			// - 180 to  + 180 ?
			angleMap.put(jointName, (float)MathUtils.radToDeg(theta)%360.0F);
{'fraction_non_alphanumeric': 0.09100418410041841, 'fraction_numerical': 0.008629707112970711, 'mean_word_length': 4.658284023668639, 'pattern_counts': {'":': 0, '<': 7, '<?xml version=': 0, '>': 7, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5825871', 'n_tokens_mistral': 1310, 'n_tokens_neox': 1189, 'n_words': 343}